318 lines
6.6 KiB
C
318 lines
6.6 KiB
C
#include "main.h"
|
|
#include "time.h"
|
|
#include "rtklib.h"
|
|
|
|
#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;
|
|
|
|
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;
|
|
}
|
|
/* get ecef position */
|
|
// sol->stat = mode == 'D' ? SOLQ_DGPS : SOLQ_SINGLE;
|
|
// sol->ns = 0;
|
|
// sol->type = 0;
|
|
// pos[0] = (ns == 'S' ? -1.0 : 1.0) * dmm2deg(lat) * D2R;
|
|
// pos[1] = (ew == 'W' ? -1.0 : 1.0) * dmm2deg(lon) * D2R;
|
|
// pos2ecef(pos, sol->rr);
|
|
/* get utc and gpst time */
|
|
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->gpsttime = utc2gpst(sol->utctime);
|
|
|
|
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, 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))
|
|
{
|
|
ans = -1;
|
|
}
|
|
else
|
|
{
|
|
return decode_nmea((char*) nmea->buff, &(nmea->sol));
|
|
}
|
|
}
|
|
|
|
return ans;
|
|
}
|