399 lines
8.3 KiB
C
399 lines
8.3 KiB
C
|
#include "main.h"
|
|||
|
|
#include "time.h"
|
|||
|
|
#include "rtklib.h"
|
|||
|
|
|
|||
|
|
/* 在rtklib 中定义结构体 */
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define MAXFIELD 64 /* max number of fields in a record */
|
|||
|
|
|
|||
|
|
typedef enum fsm_err
|
|||
|
|
{
|
|||
|
|
FSM_EOK = 0,
|
|||
|
|
FSM_ERROR,
|
|||
|
|
FSM_EINVAL,
|
|||
|
|
} fsm_err_t;
|
|||
|
|
|
|||
|
|
enum
|
|||
|
|
{
|
|||
|
|
FSM_STATE_INIT = 0,
|
|||
|
|
FSM_STATE_ABORT,
|
|||
|
|
FSM_STATE_COMPLETE,
|
|||
|
|
FSM_STATE_NAME_RECV,
|
|||
|
|
FSM_STATE_ARGS_RECV,
|
|||
|
|
FSM_STATE_CSUM_RECV,
|
|||
|
|
FSM_STATE_CRLF_RECV,
|
|||
|
|
FSM_STATE_MAX,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
FSM_EVENT_DOLLAR_CHAR = 0,
|
|||
|
|
FSM_EVENT_ORDINARY_CHAR,
|
|||
|
|
FSM_EVENT_COMMA_CHAR,
|
|||
|
|
FSM_EVENT_ASTERISK_CHAR,
|
|||
|
|
FSM_EVENT_CR_CHAR,
|
|||
|
|
FSM_EVENT_LF_CHAR,
|
|||
|
|
FSM_EVENT_NUL_CHAR,
|
|||
|
|
FSM_EVENT_MAX,
|
|||
|
|
FSM_EVENT_G_CHAR,
|
|||
|
|
FSM_EVENT_DATA,
|
|||
|
|
FSM_EVENT_CHEAK1,
|
|||
|
|
FSM_EVENT_CHEAK2,
|
|||
|
|
FSM_EVENT_FINISH,
|
|||
|
|
FSM_EVENT_IDLE,
|
|||
|
|
} fsm_event_t;
|
|||
|
|
|
|||
|
|
enum
|
|||
|
|
{
|
|||
|
|
FSM_ERROR_DOLLAR_REPEAT = 1,
|
|||
|
|
FSM_ERROR_ORDINARY_EARLY,
|
|||
|
|
FSM_ERROR_ORDINARY_REPEAT,
|
|||
|
|
FSM_ERROR_COMMA_EARLY,
|
|||
|
|
FSM_ERROR_COMMA_REPEAT,
|
|||
|
|
FSM_ERROR_ASTERISK_EARLY,
|
|||
|
|
FSM_ERROR_ASTERISK_REPEAT,
|
|||
|
|
FSM_ERROR_CR_EARLY,
|
|||
|
|
FSM_ERROR_CR_REPEAT,
|
|||
|
|
FSM_ERROR_LF_EARLY,
|
|||
|
|
FSM_ERROR_LF_REPEAT,
|
|||
|
|
FSM_ERROR_NUL_EARLY,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
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];
|
|||
|
|
}
|
|||
|
|
if (buff[++i] > '9')
|
|||
|
|
{
|
|||
|
|
ans += 10 + buff[i] - 'A';
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ans += buff[i] - '0';
|
|||
|
|
}
|
|||
|
|
ans *= 16;
|
|||
|
|
if (buff[++i] > '9')
|
|||
|
|
{
|
|||
|
|
ans += 10 + buff[i] - 'A';
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ans += buff[i] - '0';
|
|||
|
|
}
|
|||
|
|
if (ans == result)
|
|||
|
|
return 1;
|
|||
|
|
else
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
/* convert calendar day/time to time -------------------------------------------
|
|||
|
|
* convert calendar day/time to gtime_t struct
|
|||
|
|
* args : double *ep I day/time {year,month,day,hour,min,sec}
|
|||
|
|
* return : gtime_t struct
|
|||
|
|
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
|
|||
|
|
*-----------------------------------------------------------------------------*/
|
|||
|
|
extern gtime_t epoch2time(const double *ep)
|
|||
|
|
{
|
|||
|
|
const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
|
|||
|
|
gtime_t time={0};
|
|||
|
|
int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
|
|||
|
|
|
|||
|
|
if (year<1970||2099<year||mon<1||12<mon) return time;
|
|||
|
|
|
|||
|
|
/* leap year if year%4==0 in 1901-2099 */
|
|||
|
|
days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
|
|||
|
|
sec=(int)floor(ep[5]);
|
|||
|
|
time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
|
|||
|
|
time.sec=ep[5]-sec;
|
|||
|
|
return time;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 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->utctime=epoch2time(ep);
|
|||
|
|
sol->time = utc2gpst(sol->utctime);
|
|||
|
|
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 \r\n",
|
|||
|
|
// time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns,
|
|||
|
|
// vel, dir, ang, mew, mode);
|
|||
|
|
|
|||
|
|
// trace(3, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d \r\n",
|
|||
|
|
// time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns);
|
|||
|
|
|
|||
|
|
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) /* <20><><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>У<EFBFBD><D0A3> */
|
|||
|
|
// {
|
|||
|
|
// 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) /* Խ<><D4BD> */
|
|||
|
|
// {
|
|||
|
|
// ans = -1;
|
|||
|
|
// }
|
|||
|
|
// if (ans != 0)
|
|||
|
|
// {
|
|||
|
|
// nmea->len = 0;
|
|||
|
|
// nmea->nbyte = 0;
|
|||
|
|
// state = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
// return ans;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
extern int input_nmea(nmea_t *nmea, const uint8_t data)
|
|||
|
|
{
|
|||
|
|
static fsm_event_t event = 0;
|
|||
|
|
int8_t ans = 0;
|
|||
|
|
// trace(5, "input_nmea: data=%02x\n", data);
|
|||
|
|
|
|||
|
|
switch (data)
|
|||
|
|
{
|
|||
|
|
case '$':
|
|||
|
|
event = FSM_EVENT_DOLLAR_CHAR;
|
|||
|
|
break;
|
|||
|
|
case '*':
|
|||
|
|
if(event == FSM_EVENT_DATA)
|
|||
|
|
event = FSM_EVENT_ASTERISK_CHAR;
|
|||
|
|
else
|
|||
|
|
event = FSM_EVENT_IDLE;
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
switch (event)
|
|||
|
|
{
|
|||
|
|
case FSM_EVENT_DOLLAR_CHAR:
|
|||
|
|
nmea->nbyte = 0;
|
|||
|
|
nmea->len = 0;
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
event = FSM_EVENT_G_CHAR;
|
|||
|
|
break;
|
|||
|
|
case FSM_EVENT_G_CHAR:
|
|||
|
|
if (data == 'G')
|
|||
|
|
{
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
event = FSM_EVENT_DATA;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
event = FSM_EVENT_IDLE;
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
case FSM_EVENT_DATA:
|
|||
|
|
if (nmea->nbyte > 168)
|
|||
|
|
{
|
|||
|
|
event = FSM_EVENT_IDLE;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
case FSM_EVENT_ASTERISK_CHAR:
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
nmea->len = nmea->nbyte + 2;
|
|||
|
|
event = FSM_EVENT_CHEAK1;
|
|||
|
|
break;
|
|||
|
|
case FSM_EVENT_CHEAK1:
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
event = FSM_EVENT_CHEAK2;
|
|||
|
|
break;
|
|||
|
|
case FSM_EVENT_CHEAK2:
|
|||
|
|
nmea->buff[nmea->nbyte++] = data;
|
|||
|
|
event = FSM_EVENT_FINISH;
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
if (event == FSM_EVENT_FINISH)
|
|||
|
|
{
|
|||
|
|
event = FSM_EVENT_IDLE;
|
|||
|
|
if (0 == nmea_xor(nmea->buff, nmea->len))
|
|||
|
|
{
|
|||
|
|
// trace(3, "nmea parity error: len=%d \r\n", nmea->len);
|
|||
|
|
ans = -1;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ans = 1;
|
|||
|
|
return decode_nmea((char *)nmea->buff, &(nmea->sol));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return ans;
|
|||
|
|
}
|