1211 lines
38 KiB
C
1211 lines
38 KiB
C
#include "rtklib.h"
|
|
#include <rtthread.h>
|
|
#include <rtdevice.h>
|
|
#include <ulog.h>
|
|
#include <rtdef.h>
|
|
|
|
#define MIN_INT_RESET 30000 /* mininum interval of reset command (ms) */
|
|
#define LOG_TAG "ST_rtksvr.c"
|
|
|
|
|
|
|
|
/* 静态变量 ------------------------------------*/
|
|
#ifdef STATIC
|
|
#define SVR_STATIC
|
|
#endif
|
|
|
|
/* read strial -----------------------------------------------------------------
|
|
* read data from strial (unblocked)
|
|
* args : unsinged char *buff O data buffer
|
|
* int n I maximum data length
|
|
* int index I (user or refer)
|
|
* return : read data length
|
|
* notes : if no data, return immediately with no data
|
|
*-----------------------------------------------------------------------------*/
|
|
//static int read_strial(uint8_t *buff, int n, int index)
|
|
//{
|
|
// int nr = 0;
|
|
// // TODO: 从串口读取数据
|
|
// // index:{rov,base} 对应的串口
|
|
// switch (index)
|
|
// {
|
|
// case 0:
|
|
// break;
|
|
// case 1:
|
|
// break;
|
|
// default: // 正常不会进到这里
|
|
// break;
|
|
// }
|
|
// return nr;
|
|
//}
|
|
|
|
/* save output buffer --------------------------------------------------------*/
|
|
//static void saveoutbuf(rtksvr_t *svr, uint8_t *buff, int n, int index)
|
|
//{
|
|
// rtksvrlock(svr);
|
|
//
|
|
// n = n < svr->buffsize - svr->nsb[index] ? n : svr->buffsize - svr->nsb[index]; // 判断是否能完整写入
|
|
// memcpy(svr->sbuf[index] + svr->nsb[index], buff, n);
|
|
// svr->nsb[index] += n;
|
|
//
|
|
// rtksvrunlock(svr);
|
|
//}
|
|
|
|
/* write solution to output stream -------------------------------------------*/
|
|
void writesol_mini(rtksvr_t *svr, int index)
|
|
{
|
|
// solopt_t solopt = solopt_default;
|
|
uint8_t buff[MAXSOLMSG + 1];
|
|
int i=0;
|
|
int n=0;
|
|
|
|
tracet(4, "writesol: index=%d\n", index);
|
|
|
|
for (i = 0; i < 2; i++)
|
|
{
|
|
|
|
if (svr->solopt[i].posf == SOLF_STAT)
|
|
{
|
|
|
|
/* output solution status */
|
|
// rtksvrlock(svr);
|
|
n = rtkoutstat(&svr->rtk, (char*)buff);
|
|
// rtksvrunlock(svr);
|
|
}
|
|
else
|
|
{
|
|
/* output solution */
|
|
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 相关信息 */
|
|
// n = outsolexs(buff, &svr->rtk.sol, svr->rtk.ssat, svr->solopt + i);
|
|
}
|
|
}
|
|
#ifdef ENAGLO
|
|
/* update glonass frequency channel number in raw data struct ----------------*/
|
|
static void update_glofcn(rtksvr_t *svr)
|
|
{
|
|
int i, j, sat, frq;
|
|
|
|
for (i = 0; i < MAXPRNGLO; i++)
|
|
{
|
|
sat = satno(SYS_GLO, i + 1);
|
|
|
|
for (j = 0, frq = -999; j < 3; j++)
|
|
{
|
|
if (svr->raw[j].nav.geph[i].sat != sat)
|
|
continue;
|
|
frq = svr->raw[j].nav.geph[i].frq;
|
|
}
|
|
if (frq < -7 || frq > 6)
|
|
continue;
|
|
|
|
for (j = 0; j < 3; j++)
|
|
{
|
|
if (svr->raw[j].nav.geph[i].sat == sat)
|
|
continue;
|
|
svr->raw[j].nav.geph[i].sat = sat;
|
|
svr->raw[j].nav.geph[i].frq = frq;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
/* update observation data ---------------------------------------------------*/
|
|
static void update_obs(rtksvr_t *svr, obs_t *obs, int index, int iobs)
|
|
{
|
|
int i, n = 0, sat, sys;
|
|
|
|
if (iobs < MAXOBSBUF)
|
|
{
|
|
for (i = 0; i < obs->n; i++)
|
|
{
|
|
sat = obs->data[i].sat;
|
|
sys = satsys(sat, NULL);
|
|
if (svr->rtk.opt.exsats[sat - 1] == 1 || !(sys & svr->rtk.opt.navsys))
|
|
{
|
|
continue;
|
|
}
|
|
svr->obs[index][iobs].data[n] = obs->data[i];
|
|
svr->obs[index][iobs].data[n++].rcv = index + 1;
|
|
}
|
|
svr->obs[index][iobs].n = n;
|
|
sortobs(&svr->obs[index][iobs]);
|
|
}
|
|
svr->nmsg[index][0]++;
|
|
}
|
|
static void update_eph(rtksvr_t *svr, nav_t *nav, int ephsat, int ephset,
|
|
int index)
|
|
{
|
|
eph_t *eph1, *eph2;
|
|
#ifdef ENAGLO
|
|
geph_t *geph1, *geph2;
|
|
#endif
|
|
int prn;
|
|
|
|
if (satsys(ephsat, &prn) != SYS_GLO)
|
|
{
|
|
if (!svr->navsel || svr->navsel == index + 1)
|
|
{
|
|
/* svr->nav.eph={current_set1,current_set2} */
|
|
eph1 = nav->eph + ephsat - 1 + MAXSAT * ephset; /* 接收到的 */
|
|
eph2 = svr->nav.eph + ephsat - 1 + MAXSAT * ephset; /* 当前的 */
|
|
if (eph2->ttr.time == 0 || (eph1->iode != eph2->iode) || (timediff(eph1->toe, eph2->toe) != 0.0) || (timediff(eph1->toc, eph2->toc) != 0.0))
|
|
{
|
|
*eph2 = *eph1; /* received->current */
|
|
}
|
|
}
|
|
svr->nmsg[index][1]++;
|
|
}
|
|
#ifdef ENAGLO
|
|
else
|
|
{
|
|
if (!svr->navsel || svr->navsel == index + 1)
|
|
{
|
|
geph1 = nav->geph + prn - 1;
|
|
geph2 = svr->nav.geph + prn - 1;
|
|
if (geph2->tof.time == 0 ||
|
|
(geph1->iode != geph2->iode))
|
|
{
|
|
*geph2 = *geph1;
|
|
update_glofcn(svr);
|
|
}
|
|
}
|
|
svr->nmsg[index][6]++;
|
|
}
|
|
#endif
|
|
}
|
|
#ifdef ENASBS
|
|
/* update sbas message -------------------------------------------------------*/
|
|
static void update_sbs(rtksvr_t *svr, sbsmsg_t *sbsmsg, int index)
|
|
{
|
|
int i, sbssat = svr->rtk.opt.sbassatsel;
|
|
if (sbsmsg && (sbssat == sbsmsg->prn || sbssat == 0))
|
|
{
|
|
sbsmsg->rcv = index + 1;
|
|
if (svr->nsbs < MAXSBSMSG)
|
|
{
|
|
svr->sbsmsg[svr->nsbs++] = *sbsmsg;
|
|
}
|
|
else
|
|
{
|
|
for (i = 0; i < MAXSBSMSG - 1; i++)
|
|
svr->sbsmsg[i] = svr->sbsmsg[i + 1];
|
|
svr->sbsmsg[i] = *sbsmsg;
|
|
}
|
|
sbsupdatecorr(sbsmsg, &svr->nav);
|
|
}
|
|
svr->nmsg[index][3]++;
|
|
}
|
|
/* update ssr corrections ----------------------------------------------------*/
|
|
static void update_ssr(rtksvr_t *svr, int index)
|
|
{
|
|
int i, sys, prn, iode;
|
|
|
|
for (i = 0; i < MAXSAT; i++)
|
|
{
|
|
if (!svr->rtcm[index].ssr[i].update)
|
|
continue;
|
|
|
|
/* check consistency between iods of orbit and clock */
|
|
if (svr->rtcm[index].ssr[i].iod[0] != svr->rtcm[index].ssr[i].iod[1])
|
|
{
|
|
continue;
|
|
}
|
|
svr->rtcm[index].ssr[i].update = 0;
|
|
|
|
iode = svr->rtcm[index].ssr[i].iode;
|
|
sys = satsys(i + 1, &prn);
|
|
|
|
/* check corresponding ephemeris exists */
|
|
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS)
|
|
{
|
|
if (svr->nav.eph[i].iode != iode &&
|
|
svr->nav.eph[i + MAXSAT].iode != iode)
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
else if (sys == SYS_GLO)
|
|
{
|
|
if (svr->nav.geph[prn - 1].iode != iode &&
|
|
svr->nav.geph[prn - 1 + MAXPRNGLO].iode != iode)
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
svr->nav.ssr[i] = svr->rtcm[index].ssr[i];
|
|
}
|
|
svr->nmsg[index][7]++;
|
|
}
|
|
#endif
|
|
/* update ion/utc parameters -------------------------------------------------*/
|
|
static void update_ionutc(rtksvr_t *svr, nav_t *nav, int index)
|
|
{
|
|
if (svr->navsel == 0 || svr->navsel == index + 1)
|
|
{
|
|
matcpy(svr->nav.utc_gps, nav->utc_gps, 8, 1);
|
|
// matcpy(svr->nav.utc_glo, nav->utc_glo, 8, 1);
|
|
// matcpy(svr->nav.utc_gal, nav->utc_gal, 8, 1);
|
|
// matcpy(svr->nav.utc_qzs, nav->utc_qzs, 8, 1);
|
|
// matcpy(svr->nav.utc_cmp, nav->utc_cmp, 8, 1);
|
|
// matcpy(svr->nav.utc_irn, nav->utc_irn, 9, 1);
|
|
// matcpy(svr->nav.utc_sbs, nav->utc_sbs, 4, 1);
|
|
matcpy(svr->nav.ion_gps, nav->ion_gps, 8, 1);
|
|
// matcpy(svr->nav.ion_gal, nav->ion_gal, 4, 1);
|
|
// matcpy(svr->nav.ion_qzs, nav->ion_qzs, 8, 1);
|
|
// matcpy(svr->nav.ion_cmp, nav->ion_cmp, 8, 1);
|
|
// matcpy(svr->nav.ion_irn, nav->ion_irn, 8, 1);
|
|
}
|
|
svr->nmsg[index][2]++;
|
|
}
|
|
/* update antenna position ---------------------------------------------------*/
|
|
static void update_antpos(rtksvr_t *svr, int index)
|
|
{
|
|
sta_t *sta;
|
|
double pos[3], del[3] = {0}, dr[3];
|
|
int i;
|
|
|
|
if (svr->rtk.opt.refpos == POSOPT_RTCM && index == 1)
|
|
{
|
|
if (svr->format[1] == STRFMT_RTCM3)
|
|
{
|
|
sta = &svr->rtcm[1].sta;
|
|
/* update base station position */
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
svr->rtk.rb[i] = sta->pos[i];
|
|
}
|
|
/* antenna delta */
|
|
ecef2pos(svr->rtk.rb, pos);
|
|
if (sta->deltype)
|
|
{ /* xyz */
|
|
del[2] = sta->hgt;
|
|
enu2ecef(pos, del, dr);
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
svr->rtk.rb[i] += sta->del[i] + dr[i];
|
|
}
|
|
}
|
|
else
|
|
{ /* enu */
|
|
enu2ecef(pos, sta->del, dr);
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
svr->rtk.rb[i] += dr[i];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
svr->nmsg[index][4]++;
|
|
}
|
|
|
|
/* update rtk server struct --------------------------------------------------*/
|
|
// static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
|
|
// int ephsat, int ephset, sbsmsg_t *sbsmsg, int index,
|
|
// int iobs)
|
|
static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav,
|
|
int ephsat, int ephset, int index, int iobs)
|
|
{
|
|
int nbyte = 0;
|
|
tracet(4, "updatesvr: ret=%d ephsat=%d ephset=%d index=%d\n", ret, ephsat,
|
|
ephset, index);
|
|
|
|
if (ret == 1)
|
|
{ /* observation data */
|
|
update_obs(svr, obs, index, iobs);
|
|
if (index == 0)
|
|
{
|
|
nbyte = dbg_show_obs(svr->obs[0][0].data, svr->obs[0][0].n, 0, (char *)NULL);
|
|
}
|
|
}
|
|
else if (ret == 2)
|
|
{ /* ephemeris */
|
|
update_eph(svr, nav, ephsat, ephset, index);
|
|
}
|
|
// else if (ret==3) { /* sbas message */
|
|
// update_sbs(svr,sbsmsg,index);
|
|
// }
|
|
else if (ret == 9)
|
|
{ /* ion/utc parameters */
|
|
update_ionutc(svr, nav, index);
|
|
}
|
|
else if (ret == 5)
|
|
{ /* antenna postion */
|
|
update_antpos(svr, index);
|
|
}
|
|
else if (ret == 7)
|
|
{ /* dgps correction */
|
|
svr->nmsg[index][5]++;
|
|
}
|
|
// else if (ret==10) { /* ssr message */
|
|
// update_ssr(svr,index);
|
|
// }
|
|
else if (ret == -1)
|
|
{ /* error */
|
|
svr->nmsg[index][9]++;
|
|
}
|
|
}
|
|
/* decode receiver raw/rtcm data ---------------------------------------------*/
|
|
extern nmea_t nmea_pnt;
|
|
int decoderaw(rtksvr_t *svr, int index)
|
|
{
|
|
obs_t *obs;
|
|
nav_t *nav;
|
|
// sbsmsg_t *sbsmsg=NULL;
|
|
int i, ret, ephsat, ephset, fobs = 0;
|
|
// todo rtksvrlock(svr);
|
|
|
|
for (i = 0; i < svr->nb[index]; i++)
|
|
{
|
|
// 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,
|
|
10: input ssr messages) */
|
|
obs = &svr->rtcm[index].obs;
|
|
nav = &svr->rtcm[index].nav;
|
|
ephsat = svr->rtcm[index].ephsat;
|
|
// ephset = svr->rtcm[index].ephset; /* 没有使能GAL卫星 所以这个标志位永远为0 */
|
|
ephset = 0; /* 星历选择*/
|
|
|
|
/* update rtk server */
|
|
if (ret > 0)
|
|
{
|
|
// update_svr(svr, ret, obs, nav, ephsat, ephset, sbsmsg, index, fobs);
|
|
update_svr(svr, ret, obs, nav, ephsat, ephset, index, fobs);
|
|
}
|
|
/* observation data received */
|
|
if (ret == 1) /* 接受一次完整的观测值电文 */
|
|
{
|
|
if (fobs < MAXOBSBUF)
|
|
fobs++;
|
|
else
|
|
svr->prcout++;
|
|
}
|
|
}
|
|
svr->nb[index] = 0;
|
|
|
|
// rtksvrunlock(svr);
|
|
|
|
return fobs;
|
|
}
|
|
/* carrier-phase bias (fcb) correction ---------------------------------------*/
|
|
// static void corr_phase_bias(obsd_t *obs, int n, const nav_t *nav)
|
|
//{
|
|
// double freq;
|
|
// uint8_t code;
|
|
// int i,j;
|
|
//
|
|
// for (i=0;i<n;i++) for (j=0;j<NFREQ;j++) {
|
|
// code=obs[i].code[j];
|
|
// if ((freq=sat2freq(obs[i].sat,code,nav))==0.0) continue;
|
|
//
|
|
// /* correct phase bias (cyc) */
|
|
// obs[i].L[j]-=nav->ssr[obs[i].sat-1].pbias[code-1]*freq/CLIGHT;
|
|
// }
|
|
// }
|
|
/* periodic command ----------------------------------------------------------*/
|
|
static void periodic_cmd(int cycle, const char *cmd, stream_t *stream)
|
|
{
|
|
const char *p = cmd, *q;
|
|
char msg[1024], *r;
|
|
int n, period;
|
|
|
|
for (p = cmd;; p = q + 1)
|
|
{
|
|
for (q = p;; q++)
|
|
if (*q == '\r' || *q == '\n' || *q == '\0')
|
|
break;
|
|
n = (int)(q - p);
|
|
strncpy(msg, p, n);
|
|
msg[n] = '\0';
|
|
|
|
period = 0;
|
|
if ((r = strrchr(msg, '#')))
|
|
{
|
|
sscanf(r, "# %d", &period);
|
|
*r = '\0';
|
|
while (*--r == ' ')
|
|
*r = '\0'; /* delete tail spaces */
|
|
}
|
|
if (period <= 0)
|
|
period = 1000;
|
|
if (*msg && cycle % period == 0)
|
|
{
|
|
strsendcmd(stream, msg);
|
|
}
|
|
if (!*q)
|
|
break;
|
|
}
|
|
}
|
|
/* baseline length -----------------------------------------------------------*/
|
|
static double baseline_len(const rtk_t *rtk)
|
|
{
|
|
double dr[3];
|
|
int i;
|
|
|
|
if (norm(rtk->sol.rr, 3) <= 0.0 || norm(rtk->rb, 3) <= 0.0)
|
|
return 0.0;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
{
|
|
dr[i] = rtk->sol.rr[i] - rtk->rb[i];
|
|
}
|
|
return norm(dr, 3) * 0.001; /* (km) */
|
|
}
|
|
/* send nmea request to base/nrtk input stream -------------------------------*/
|
|
//todo static void send_nmea(rtksvr_t *svr, uint32_t *tickreset)
|
|
//{
|
|
// sol_t sol_nmea = {{0}};
|
|
// double vel, bl;
|
|
// uint32_t tick = tickget();
|
|
// int i;
|
|
//
|
|
//// todo if (svr->stream[1].state != 1)
|
|
//// return;
|
|
// sol_nmea.ns = 10; /* Some servers don't like when ns = 0 */
|
|
//
|
|
// if (svr->nmeareq == 1)
|
|
// { /* lat-lon-hgt mode */
|
|
// sol_nmea.stat = SOLQ_SINGLE;
|
|
// sol_nmea.time = utc2gpst(timeget());
|
|
// matcpy(sol_nmea.rr, svr->nmeapos, 3, 1);
|
|
////todo strsendnmea(svr->stream + 1, &sol_nmea);
|
|
// }
|
|
// else if (svr->nmeareq == 2)
|
|
// { /* single-solution mode */
|
|
// if (norm(svr->rtk.sol.rr, 3) <= 0.0)
|
|
// return;
|
|
// sol_nmea.stat = SOLQ_SINGLE;
|
|
// sol_nmea.time = utc2gpst(timeget());
|
|
// matcpy(sol_nmea.rr, svr->rtk.sol.rr, 3, 1);
|
|
//// todo strsendnmea(svr->stream + 1, &sol_nmea);
|
|
// }
|
|
// else if (svr->nmeareq == 3)
|
|
// { /* reset-and-single-sol mode */
|
|
//
|
|
// /* send reset command if baseline over threshold */
|
|
// bl = baseline_len(&svr->rtk);
|
|
// if (bl >= svr->bl_reset && (int)(tick - *tickreset) > MIN_INT_RESET)
|
|
// {
|
|
// // strsendcmd(svr->stream+1,svr->cmd_reset);
|
|
//
|
|
// tracet(2, "send reset: bl=%.3f rr=%.3f %.3f %.3f rb=%.3f %.3f %.3f\n",
|
|
// bl, svr->rtk.sol.rr[0], svr->rtk.sol.rr[1], svr->rtk.sol.rr[2],
|
|
// svr->rtk.rb[0], svr->rtk.rb[1], svr->rtk.rb[2]);
|
|
// *tickreset = tick;
|
|
// }
|
|
// if (norm(svr->rtk.sol.rr, 3) <= 0.0)
|
|
// return;
|
|
// sol_nmea.stat = SOLQ_SINGLE;
|
|
// sol_nmea.time = utc2gpst(timeget());
|
|
// matcpy(sol_nmea.rr, svr->rtk.sol.rr, 3, 1);
|
|
//
|
|
// /* set predicted position if velocity > 36km/h */
|
|
// if ((vel = norm(svr->rtk.sol.rr + 3, 3)) > 10.0)
|
|
// {
|
|
// for (i = 0; i < 3; i++)
|
|
// {
|
|
// sol_nmea.rr[i] += svr->rtk.sol.rr[i + 3] / vel * svr->bl_reset * 0.8;
|
|
// }
|
|
// }
|
|
////todo strsendnmea(svr->stream + 1, &sol_nmea);
|
|
//
|
|
// tracet(3, "send nmea: rr=%.3f %.3f %.3f\n", sol_nmea.rr[0], sol_nmea.rr[1],
|
|
// sol_nmea.rr[2]);
|
|
// }
|
|
//}
|
|
/* rtk server mini thread ---------------------------------------------------------*/
|
|
|
|
//static void *rtksvrthread_mini(void *arg)
|
|
//{
|
|
// rtksvr_t *svr = (rtksvr_t *)arg;
|
|
// obs_t obs; /* 28 B */
|
|
// obsd_t data[MAXOBS * 2]; /* 7616 B */
|
|
// sol_t sol = {{0}}; /* 199 B */
|
|
// double tt;
|
|
// uint32_t tick, ticknmea, tick1hz, tickreset;
|
|
// uint8_t *p, *q;
|
|
// int i, j, n = 0, fobs[2] = {0}, cycle, cputime;
|
|
// uint8_t nr, nu;
|
|
// tracet(3, "rtksvrthread:\n");
|
|
//
|
|
// svr->state = 1;
|
|
// obs.data = data;
|
|
// svr->tick = tickget();
|
|
// ticknmea = tick1hz = svr->tick - 1000;
|
|
// tickreset = svr->tick - MIN_INT_RESET;
|
|
//
|
|
// while(1)
|
|
// {
|
|
//
|
|
//
|
|
// tick = tickget();
|
|
// for (i = 0; i < 2; i++)
|
|
// {
|
|
// p = svr->buff[i] + svr->nb[i];
|
|
// q = svr->buff[i] + svr->buffsize;
|
|
//
|
|
// /* 读取基准站/接收机发送来的消息 */
|
|
// if ((n = read_strial(p, q - p, i)) <= 0)
|
|
// continue;
|
|
// svr->nb[i] += n;
|
|
// }
|
|
// /* 解码基准站/接收机的消息 解码后数据保存在RTCM[index] 结构中 再更新到svr->obs */
|
|
// for (i = 0; i < 2; i++)
|
|
// {
|
|
// fobs[i] = decoderaw(svr, i); //传入svr->buff[i] 解算成功后会对 svr->nb[i] 清零
|
|
// }
|
|
// /* 将基准站和移动站的观测数据合二为一 这里重新申请了一个MAXOBS*2 的数组, 我觉得是不是有点问题? */
|
|
// for (i = 0; i < fobs[0]; i++)
|
|
// {
|
|
// obs.n = 0;
|
|
// nr = nu = 0;
|
|
// for (j = 0; j < svr->obs[0][i].n && obs.n < MAXOBS * 2; j++)
|
|
// {
|
|
// obs.data[obs.n++] = svr->obs[0][i].data[j];
|
|
// nu++;
|
|
// }
|
|
//
|
|
// for (j = 0; j < svr->obs[1][0].n && obs.n < MAXOBS * 2; j++)
|
|
// {
|
|
// obs.data[obs.n++] = svr->obs[1][0].data[j];
|
|
// nr++;
|
|
// }
|
|
//
|
|
// /* rtk positioning */
|
|
// rtksvrlock(svr);
|
|
// rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav);
|
|
//
|
|
// rtksvrunlock(svr);
|
|
//
|
|
// /* 如果解算结果有效, 设置解算时间点 */
|
|
// if (svr->rtk.sol.stat != SOLQ_NONE)
|
|
// {
|
|
// /* adjust current time */
|
|
// tt = (int)(tickget() - tick) / 1000.0 + DTTOL;
|
|
// timeset(gpst2utc(timeadd(svr->rtk.sol.time, tt)));
|
|
//
|
|
// /* write solution */
|
|
// writesol_mini(svr, i);
|
|
// }
|
|
// /* 如果设定的运算时间到了发现还没有计算完, 则记录没有计算完的观测次数 */
|
|
// if ((int)(tickget() - tick) >= svr->cycle)
|
|
// {
|
|
// svr->prcout += fobs[0] - i - 1;
|
|
// }
|
|
// }
|
|
// /* send null solution if no solution (1hz) */
|
|
// if (svr->rtk.sol.stat == SOLQ_NONE && (int)(tick - tick1hz) >= 1000)
|
|
// {
|
|
// writesol_mini(svr, 0);
|
|
// tick1hz = tick;
|
|
// }
|
|
// /* send nmea request to base/nrtk input stream */
|
|
// if (svr->nmeacycle > 0 && (int)(tick - ticknmea) >= svr->nmeacycle)
|
|
// {
|
|
// // TODO: 完成send_nmea 到到串口的移植
|
|
// // send_nmea(svr, &tickreset);
|
|
// ticknmea = tick;
|
|
// }
|
|
// cputime = (int)(tickget() - tick);
|
|
// if (cputime > 0)
|
|
// svr->cputime = cputime;
|
|
//
|
|
// /* sleep until next cycle */
|
|
// sleepms(svr->cycle - cputime);
|
|
// }
|
|
// /* 关闭已经打开的流 */
|
|
//// for (i = 0; i < MAXSTRRTK; i++)
|
|
//// // TODO: 这里应该是关闭嵌入式的数据流 例如接收机的串口 移动站的串口
|
|
//// // strclose(svr->stream + i);
|
|
//// for (i = 0; i < 2; i++)
|
|
//// {
|
|
//// svr->nb[i] = 0;
|
|
////// svr->npb[i] = 0;
|
|
//// free(svr->buff[i]);
|
|
//// svr->buff[i] = NULL;
|
|
//// // free(svr->pbuf[i]);
|
|
//// // svr->pbuf[i] = NULL;
|
|
//// // free_raw (svr->raw +i);
|
|
//// free_rtcm(svr->rtcm + i);
|
|
//// }
|
|
// // for (i = 0; i < 2; i++)
|
|
// // {
|
|
// // svr->nsb[i] = 0;
|
|
// // free(svr->sbuf[i]);
|
|
// // svr->sbuf[i] = NULL;
|
|
// // }
|
|
// return 0;
|
|
//}
|
|
/* initialize rtk server -------------------------------------------------------
|
|
* initialize rtk server
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* return : status (0:error,1:ok)
|
|
*-----------------------------------------------------------------------------*/
|
|
int rtksvrinit(rtksvr_t *svr)
|
|
{
|
|
gtime_t time0 = {0};
|
|
sol_t sol0 = {{0}};
|
|
eph_t eph0 = {0, -1, -1};
|
|
int i, j;
|
|
|
|
tracet(3, "rtksvrinit:\n");
|
|
|
|
rtkinit(&svr->rtk, &prcopt_default); /* 初始化了RTK需要的数组 */
|
|
svr->state = svr->cycle = svr->nmeacycle = svr->nmeareq = 0;
|
|
for (i = 0; i < 3; i++)
|
|
svr->nmeapos[i] = 0.0; /* 初始化NMEA坐标 */
|
|
svr->buffsize = 0;
|
|
for (i = 0; i < 2; i++)
|
|
svr->format[i] = 0; /* init 格式 */
|
|
for (i = 0; i < 2; i++)
|
|
svr->solopt[i] = solopt_default; /* init 输出格式 */
|
|
svr->navsel = svr->nsol = 0;
|
|
// svr->nsbs = 0;
|
|
for (i = 0; i < 2; i++)
|
|
svr->nb[i] = 0;
|
|
// for (i = 0; i < 2; i++)
|
|
// svr->nsb[i] = 0;
|
|
// for (i = 0; i < 2; i++)
|
|
// svr->npb[i] = 0;
|
|
for (i = 0; i < 2; i++)
|
|
svr->buff[i] = NULL;
|
|
// for (i = 0; i < 2; i++)
|
|
// svr->sbuf[i] = NULL;
|
|
// for (i = 0; i < 2; i++)
|
|
// svr->pbuf[i] = NULL;
|
|
for (i = 0; i < MAXSOLBUF; i++)
|
|
svr->solbuf[i] = sol0;
|
|
for (i = 0; i < 2; i++)
|
|
for (j = 0; j < 10; j++)
|
|
svr->nmsg[i][j] = 0;
|
|
|
|
svr->tick = 0;
|
|
//todo svr->thread = 0;
|
|
svr->cputime = svr->prcout = svr->nave = 0;
|
|
for (i = 0; i < 3; i++)
|
|
svr->rb_ave[i] = 0.0;
|
|
|
|
memset(&svr->nav, 0, sizeof(nav_t));
|
|
|
|
#ifdef SVR_STATIC
|
|
extern eph_t svr_eph[MAXSAT];
|
|
extern obsd_t svr_obsd[2][MAXOBS];
|
|
|
|
svr->nav.eph = svr_eph;
|
|
for (i = 0; i < MAXSAT * 1; i++)
|
|
svr->nav.eph[i] = eph0;
|
|
svr->nav.n = MAXSAT * 1;
|
|
#ifdef ENAGLO
|
|
extern geph_t svr_geph[NSATGLO];
|
|
geph_t geph0 = {0, -1};
|
|
svr->nav.geph = svr_geph;
|
|
for (i = 0; i < NSATGLO * 1; i++)
|
|
svr->nav.geph[i] = geph0;
|
|
svr->nav.ng = NSATGLO * 1;
|
|
#endif
|
|
#ifdef ENASBS
|
|
extern seph_t svr_seph[NSATSBS];
|
|
seph_t seph0 = {0};
|
|
svr->nav.seph = svr_seph;
|
|
for (i = 0; i < NSATSBS * 1; i++)
|
|
svr->nav.seph[i] = seph0;
|
|
svr->nav.ns = NSATSBS * 1;
|
|
#endif
|
|
for (i = 0; i < 2; i++)
|
|
for (j = 0; j < MAXOBSBUF; j++)
|
|
{
|
|
svr->obs[i][j].data = &(svr_obsd[i][j]);
|
|
}
|
|
#else
|
|
if (!(svr->nav.eph = (eph_t *)rt_malloc(sizeof(eph_t) * MAXSAT * 1)))
|
|
{
|
|
tracet(1, "rtksvrinit: malloc error\n");
|
|
return 0;
|
|
}
|
|
for (i = 0; i < MAXSAT * 1; i++)
|
|
svr->nav.eph[i] = eph0;
|
|
svr->nav.n = MAXSAT * 1;
|
|
#ifdef ENAGLO
|
|
geph_t geph0 = {0, -1};
|
|
if (!(svr->nav.geph = (geph_t *)rt_malloc(sizeof(geph_t) * NSATGLO * 1)))
|
|
{
|
|
tracet(1, "rtksvrinit: malloc error\n");
|
|
return 0;
|
|
}
|
|
for (i = 0; i < NSATGLO * 1; i++)
|
|
svr->nav.geph[i] = geph0;
|
|
svr->nav.ng = NSATGLO * 1;
|
|
#endif
|
|
#ifdef ENASBS
|
|
seph_t seph0 = {0};
|
|
if (!(svr->nav.seph = (seph_t *)rt_malloc(sizeof(seph_t) * NSATSBS * 1)))
|
|
{
|
|
tracet(1, "rtksvrinit: malloc error\n");
|
|
return 0;
|
|
}
|
|
for (i = 0; i < NSATSBS * 1; i++)
|
|
svr->nav.seph[i] = seph0;
|
|
svr->nav.ns = NSATSBS * 1;
|
|
#endif
|
|
|
|
for (i = 0; i < 2; i++)
|
|
for (j = 0; j < MAXOBSBUF; j++)
|
|
{
|
|
if (!(svr->obs[i][j].data = (obsd_t *)rt_malloc(sizeof(obsd_t) * MAXOBS)))
|
|
{
|
|
tracet(1, "rtksvrinit: malloc error\n");
|
|
return 0;
|
|
}
|
|
}
|
|
#endif
|
|
for (i = 0; i < 2; i++)
|
|
{
|
|
// memset(svr->raw +i,0,sizeof(raw_t ));
|
|
memset(svr->rtcm + i, 0, sizeof(rtcm_t));
|
|
}
|
|
//todo for (i = 0; i < MAXSTRRTK; i++)
|
|
// strinit(svr->stream + i);
|
|
|
|
// for (i=0;i<2;i++) *svr->cmds_periodic[i]='\0';x
|
|
//*svr->cmd_reset='\0';
|
|
// for (i=0;i<3;i++) svr->ftime[i]=time0;
|
|
// for (i=0;i<2;i++) svr->files[i][0]='\0';
|
|
// svr->moni=NULL;
|
|
svr->bl_reset = 10.0;
|
|
//todo initlock(&svr->lock);
|
|
|
|
return 1;
|
|
}
|
|
/* free rtk server -------------------------------------------------------------
|
|
* free rtk server
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
extern void rtksvrfree(rtksvr_t *svr)
|
|
{
|
|
|
|
#ifdef SVR_STATIC
|
|
memset(svr_obsd, 0, sizeof(obsd_t) * 2 * MAXOBSBUF); // 尚未测试 2022年6月16日
|
|
|
|
// 如果是动态申请的
|
|
#else
|
|
int i, j;
|
|
free(svr->nav.eph);
|
|
svr->nav.eph = NULL;
|
|
#ifdef ENAGLO
|
|
free(svr->nav.geph);
|
|
svr->nav.geph = NULL;
|
|
#endif
|
|
#ifdef ENASBS
|
|
free(svr->nav.seph);
|
|
svr->nav.seph = NULL;
|
|
#endif
|
|
for (i = 0; i < 3; i++)
|
|
for (j = 0; j < MAXOBSBUF; j++)
|
|
{
|
|
free(svr->obs[i][j].data);
|
|
}
|
|
#endif
|
|
rtkfree(&svr->rtk);
|
|
}
|
|
/* lock/unlock rtk server ------------------------------------------------------
|
|
* lock/unlock rtk server
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* return : status (1:ok 0:error)
|
|
*-----------------------------------------------------------------------------*/
|
|
/* start rtk server mini ------------------------------------------------------------
|
|
* start rtk server thread
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* int cycle I server cycle (ms)
|
|
* int buffsize I input buffer size (bytes)
|
|
* int *format I input stream formats (STRFMT_???)
|
|
* format[0]=input stream rover
|
|
* format[1]=input stream base station
|
|
* int nmeacycle I nmea request cycle (ms) (0:no request)
|
|
* int nmeareq I nmea request type
|
|
* (0:no,1:base pos,2:single sol,3:reset and single)
|
|
* double *nmeapos I transmitted nmea position (ecef) (m)
|
|
* prcopt_t *prcopt I rtk processing options
|
|
* solopt_t *solopt I solution options
|
|
* solopt[0]=solution 1 options
|
|
* solopt[1]=solution 2 options
|
|
* return : status (1:ok 0:error)
|
|
* 开始RTK服务线程
|
|
*-----------------------------------------------------------------------------*/
|
|
extern int rtksvrstart_mini(rtksvr_t *svr, int cycle, int buffsize, int *formats,
|
|
int nmeacycle, int nmeareq, const double *nmeapos,
|
|
prcopt_t *prcopt, solopt_t *solopt)
|
|
{
|
|
gtime_t time, time0 = {0};
|
|
int i, j;
|
|
|
|
/* STEP-1 初始化并连接结构体指针和变量 */
|
|
|
|
svr->cycle = cycle > 1 ? cycle : 1;
|
|
svr->nmeacycle = nmeacycle > 1000 ? nmeacycle : 1000;
|
|
svr->nmeareq = nmeareq;
|
|
for (i = 0; i < 3; i++)
|
|
svr->nmeapos[i] = nmeapos[i];
|
|
svr->buffsize = buffsize > 4096 ? buffsize : 4096;
|
|
for (i = 0; i < 2; i++)
|
|
svr->format[i] = formats[i];
|
|
svr->navsel = 0; /* navigation message select (0:rover,1:base,3:all) */
|
|
// svr->nsbs = 0; /* number of sbas message */
|
|
svr->nsol = 0; /* number of solution buffer */
|
|
svr->prcout = 0; /* missing observation data count */
|
|
// 重复释放
|
|
// rtkfree(&svr->rtk);
|
|
|
|
rtkinit(&svr->rtk, prcopt);
|
|
#ifdef STATIC
|
|
init_rtcm_static(svr->rtcm + 0, 0); /* 初始化移动站数组 */
|
|
init_rtcm_static(svr->rtcm + 1, 1); /* 初始化基准站数组 */
|
|
#endif
|
|
if (prcopt->initrst)
|
|
{ /* init averaging pos by restart */
|
|
svr->nave = 0;
|
|
for (i = 0; i < 3; i++)
|
|
svr->rb_ave[i] = 0.0;
|
|
}
|
|
|
|
/* STEP-2 初始化数据缓存buff */
|
|
for (i = 0; i < 2; i++)
|
|
{ /* input/log streams */
|
|
svr->nb[i] = 0;
|
|
// svr->npb[i] = 0;
|
|
if ((svr->buff[i] = (uint8_t *)rt_malloc(buffsize))==RT_NULL)
|
|
{
|
|
LOG_E("rtksvrstart: buff malloc error");
|
|
return 0;
|
|
}
|
|
for (j = 0; j < 10; j++)
|
|
svr->nmsg[i][j] = 0;
|
|
for (j = 0; j < MAXOBSBUF; j++)
|
|
svr->obs[i][j].n = 0;
|
|
init_rtcm(&svr->rtcm[i]);
|
|
}
|
|
|
|
/* set solution options */
|
|
for (i = 0; i < 2; i++)
|
|
{
|
|
svr->solopt[i] = solopt[i];
|
|
}
|
|
/* set base station position */
|
|
if (prcopt->refpos != POSOPT_SINGLE)
|
|
{
|
|
for (i = 0; i < 6; i++)
|
|
svr->rtk.rb[i] = i < 3 ? prcopt->rb[i] : 0.0;
|
|
}
|
|
/* update navigation data */
|
|
for (i = 0; i < MAXSAT * 1; i++)
|
|
svr->nav.eph[i].ttr = time0;
|
|
#ifdef ENAGLO
|
|
for (i = 0; i < NSATGLO * 1; i++)
|
|
svr->nav.geph[i].tof = time0;
|
|
#endif
|
|
#ifdef ENASBS
|
|
for (i = 0; i < NSATSBS * 1; i++)
|
|
svr->nav.seph[i].tof = time0;
|
|
#endif
|
|
|
|
/* STEP-3 打开基准站和移动站的输入流 */
|
|
// TODO: 嵌入式平台进行输入流的初始化 或者打开输入流开关 如串口
|
|
for (i = 0; i < 8; i++)
|
|
{
|
|
/* set initial time for rtcm and raw */
|
|
if (i < 2)
|
|
{
|
|
time = utc2gpst(timeget());
|
|
svr->rtcm[i].time = time;
|
|
}
|
|
}
|
|
/* STEP-4 打开处理线程 */
|
|
// TODO: 嵌入式系统打开对应线程, 创建对应线程
|
|
// if (!(svr->thread = CreateThread(NULL, 0, rtksvrthread_mini, svr, 0, NULL)))
|
|
// {
|
|
// for (i = 0; i < MAXSTRRTK; i++)
|
|
// strclose(svr->stream + i);
|
|
// return 0;
|
|
// }
|
|
return 1;
|
|
}
|
|
/* stop rtk server -------------------------------------------------------------
|
|
* start rtk server thread
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* char **cmds I input stream stop commands
|
|
* cmds[0]=input stream rover (NULL: no command)
|
|
* cmds[1]=input stream base (NULL: no command)
|
|
* cmds[2]=input stream ephem (NULL: no command)
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
extern void rtksvrstop(rtksvr_t *svr, char **cmds)
|
|
{
|
|
int i;
|
|
|
|
tracet(3, "rtksvrstop:\n");
|
|
|
|
/* write stop commands to input streams */
|
|
//todo rtksvrlock(svr);
|
|
// for (i = 0; i < 3; i++)
|
|
// {
|
|
// if (cmds[i])
|
|
// strsendcmd(svr->stream + i, cmds[i]);
|
|
// }
|
|
rtksvrunlock(svr);
|
|
|
|
/* stop rtk server */
|
|
svr->state = 0;
|
|
|
|
/* free rtk server thread */
|
|
// TODO: 嵌入式系统支持 等待线程结束
|
|
// #ifdef WIN32
|
|
// WaitForSingleObject(svr->thread, 10000);
|
|
// CloseHandle(svr->thread);
|
|
// #else
|
|
// pthread_join(svr->thread, NULL);
|
|
// #endif
|
|
}
|
|
/* open output/log stream ------------------------------------------------------
|
|
* open output/log stream
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* int index I output/log stream index
|
|
* (3:solution 1,4:solution 2,5:log rover,
|
|
* 6:log base station,7:log correction)
|
|
* int str I output/log stream types (STR_???)
|
|
* char *path I output/log stream path
|
|
* solopt_t *solopt I solution options
|
|
* return : status (1:ok 0:error)
|
|
*-----------------------------------------------------------------------------*/
|
|
extern int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
|
|
const solopt_t *solopt)
|
|
{
|
|
// TODO: 添加SD支持
|
|
// tracet(3, "rtksvropenstr: index=%d str=%d path=%s\n", index, str, path);
|
|
|
|
// if (index < 3 || index > 7 || !svr->state)
|
|
// return 0;
|
|
|
|
// rtksvrlock(svr);
|
|
|
|
// if (svr->stream[index].state > 0)
|
|
// {
|
|
// rtksvrunlock(svr);
|
|
// return 0;
|
|
// }
|
|
// if (!stropen(svr->stream + index, str, STR_MODE_W, path))
|
|
// {
|
|
// tracet(2, "stream open error: index=%d\n", index);
|
|
// rtksvrunlock(svr);
|
|
// return 0;
|
|
// }
|
|
// if (index <= 4)
|
|
// {
|
|
// svr->solopt[index - 3] = *solopt;
|
|
|
|
// /* write solution header to solution stream */
|
|
// writesolhead(svr->stream + index, svr->solopt + index - 3);
|
|
// }
|
|
// rtksvrunlock(svr);
|
|
return 1;
|
|
}
|
|
/* close output/log stream -----------------------------------------------------
|
|
* close output/log stream
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* int index I output/log stream index
|
|
* (3:solution 1,4:solution 2,5:log rover,
|
|
* 6:log base station,7:log correction)
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
extern void rtksvrclosestr(rtksvr_t *svr, int index)
|
|
{
|
|
// TODO: 添加SD支持
|
|
// tracet(3, "rtksvrclosestr: index=%d\n", index);
|
|
|
|
// if (index < 3 || index > 7 || !svr->state)
|
|
// return;
|
|
|
|
// rtksvrlock(svr);
|
|
|
|
// strclose(svr->stream + index);
|
|
|
|
// rtksvrunlock(svr);
|
|
}
|
|
/* get observation data status -------------------------------------------------
|
|
* get current observation data status
|
|
* args : rtksvr_t *svr I rtk server
|
|
* int rcv I receiver (0:rover,1:base,2:ephem)
|
|
* gtime_t *time O time of observation data
|
|
* int *sat O satellite prn numbers
|
|
* double *az O satellite azimuth angles (rad)
|
|
* double *el O satellite elevation angles (rad)
|
|
* int **snr O satellite snr for each freq (dBHz)
|
|
* snr[i][j] = sat i freq j snr
|
|
* int *vsat O valid satellite flag
|
|
* return : number of satellites
|
|
*-----------------------------------------------------------------------------*/
|
|
extern int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
|
|
double *az, double *el, int **snr, int *vsat)
|
|
{
|
|
int i, j, ns;
|
|
|
|
tracet(4, "rtksvrostat: rcv=%d\n", rcv);
|
|
|
|
if (!svr->state)
|
|
return 0;
|
|
rtksvrlock(svr);
|
|
ns = svr->obs[rcv][0].n;
|
|
if (ns > 0)
|
|
{
|
|
*time = svr->obs[rcv][0].data[0].time;
|
|
}
|
|
for (i = 0; i < ns; i++)
|
|
{
|
|
sat[i] = svr->obs[rcv][0].data[i].sat;
|
|
az[i] = svr->rtk.ssat[sat[i] - 1].azel[0];
|
|
el[i] = svr->rtk.ssat[sat[i] - 1].azel[1];
|
|
for (j = 0; j < NFREQ; j++)
|
|
{
|
|
snr[i][j] = (int)(svr->obs[rcv][0].data[i].SNR[j] * SNR_UNIT + 0.5);
|
|
}
|
|
if (svr->rtk.sol.stat == SOLQ_NONE || svr->rtk.sol.stat == SOLQ_SINGLE)
|
|
{
|
|
vsat[i] = svr->rtk.ssat[sat[i] - 1].vs;
|
|
}
|
|
else
|
|
{
|
|
vsat[i] = svr->rtk.ssat[sat[i] - 1].vsat[0];
|
|
}
|
|
}
|
|
rtksvrunlock(svr);
|
|
return ns;
|
|
}
|
|
/* get stream status -----------------------------------------------------------
|
|
* get current stream status
|
|
* args : rtksvr_t *svr I rtk server
|
|
* int *sstat O status of streams
|
|
* char *msg O status messages
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
extern void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
|
|
{
|
|
int i;
|
|
char s[MAXSTRMSG], *p = msg;
|
|
|
|
tracet(4, "rtksvrsstat:\n");
|
|
|
|
rtksvrlock(svr);
|
|
for (i = 0; i < MAXSTRRTK; i++)
|
|
{
|
|
// TODO: 获取数据流状态 或许要添加移动站情况?
|
|
// sstat[i] = strstat(svr->stream + i, s);
|
|
// strstat() 函数的返回值 return : status (-1:error,0:close,1:wait,2:connect,3:active)
|
|
if (*s)
|
|
p += sprintf(p, "(%d) %s ", i + 1, s);
|
|
}
|
|
rtksvrunlock(svr);
|
|
}
|
|
/* mark current position -------------------------------------------------------
|
|
* open output/log stream
|
|
* args : rtksvr_t *svr IO rtk server
|
|
* char *name I marker name
|
|
* char *comment I comment string
|
|
* return : status (1:ok 0:error)
|
|
*-----------------------------------------------------------------------------*/
|
|
extern int rtksvrmark(rtksvr_t *svr, const char *name, const char *comment)
|
|
{
|
|
char buff[MAXSOLMSG + 1], tstr[32], *p, *q;
|
|
double tow, pos[3];
|
|
int i, sum, week;
|
|
|
|
tracet(4, "rtksvrmark:name=%s comment=%s\n", name, comment);
|
|
|
|
if (!svr->state)
|
|
return 0;
|
|
|
|
rtksvrlock(svr);
|
|
|
|
time2str(svr->rtk.sol.time, tstr, 3);
|
|
tow = time2gpst(svr->rtk.sol.time, &week);
|
|
ecef2pos(svr->rtk.sol.rr, pos);
|
|
|
|
for (i = 0; i < 2; i++)
|
|
{
|
|
p = buff;
|
|
if (svr->solopt[i].posf == SOLF_STAT)
|
|
{
|
|
p += sprintf(p, "$MARK,%d,%.3f,%d,%.4f,%.4f,%.4f,%s,%s\r\n", week, tow,
|
|
svr->rtk.sol.stat, svr->rtk.sol.rr[0], svr->rtk.sol.rr[1],
|
|
svr->rtk.sol.rr[2], name, comment);
|
|
}
|
|
else if (svr->solopt[i].posf == SOLF_NMEA)
|
|
{
|
|
p += sprintf(p, "$GPTXT,01,01,02,MARK:%s,%s,%.9f,%.9f,%.4f,%d,%s",
|
|
name, tstr, pos[0] * R2D, pos[1] * R2D, pos[2], svr->rtk.sol.stat,
|
|
comment);
|
|
for (q = (char *)buff + 1, sum = 0; *q; q++)
|
|
sum ^= *q; /* check-sum */
|
|
p += sprintf(p, "*%02X\r\n", sum);
|
|
}
|
|
else
|
|
{
|
|
p += sprintf(p, "%s MARK: %s,%s,%.9f,%.9f,%.4f,%d,%s\r\n", COMMENTH,
|
|
name, tstr, pos[0] * R2D, pos[1] * R2D, pos[2], svr->rtk.sol.stat,
|
|
comment);
|
|
}
|
|
// TODO: 数据流支持
|
|
// strwrite(svr->stream + i + 3, (uint8_t *)buff, (int)(p - buff));
|
|
// saveoutbuf(svr, (uint8_t *)buff, (int)(p - buff), i);
|
|
}
|
|
rtksvrunlock(svr);
|
|
return 1;
|
|
}
|
|
/* 输出由RTCM电文转换的观测值内容. ----------------------------------------------
|
|
* 将RTCM电文转换成为obs,在串口中输出rinex格式的观测值内容
|
|
* args : obsd_t *obs I 当前时刻的观测值结构体
|
|
* int nobs I 当前时刻观测值的个数
|
|
* char mode I 输出的格式(0:L1 1:L1+L2 2:L1+L5)
|
|
* return :
|
|
* notes :
|
|
*-----------------------------------------------------------------------------*/
|
|
extern int dbg_show_obs(const obsd_t *obs, int nobs, char mode, char *buffer)
|
|
{
|
|
char str[128], id[16];
|
|
int i, n = 0;
|
|
|
|
time2str(obs[i].time, str, 3);
|
|
// SEGGER_RTT_printf(0, RTT_CTRL_TEXT_BRIGHT_WHITE "%s\n", str);
|
|
LOG_D("%s\n",str);
|
|
n = sprintf(buffer, "%s\r\n", str);
|
|
for (i = 0; i < nobs; i++)
|
|
{
|
|
satno2id(obs[i].sat, id);
|
|
switch (mode)
|
|
{
|
|
case 0:
|
|
n += sprintf(str, "%2d %3s %13.3f %13.3f %4.3f %3.3f\n", i, id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT);
|
|
LOG_D("%s", str);
|
|
// n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f\n", id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT);
|
|
break;
|
|
// case 1:
|
|
// n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f %13.3f %13.3f %4.3f %3.3f\n",
|
|
// id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT,
|
|
// obs[i].L[1], obs[i].P[1], obs[i].D[1], obs[i].SNR[1] * SNR_UNIT);
|
|
// break;
|
|
// case 2:
|
|
// n += sprintf(buffer + n, "%3s %13.3f %13.3f %4.3f %3.3f %13.3f %13.3f %4.3f %3.3f\n",
|
|
// id, obs[i].L[0], obs[i].P[0], obs[i].D[0], obs[i].SNR[0] * SNR_UNIT,
|
|
// obs[i].L[2], obs[i].P[2], obs[i].D[2], obs[i].SNR[2] * SNR_UNIT);
|
|
// break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
return n;
|
|
}
|