RTK_base/RTK/nmea_zjut.c

#include "rtklib.h"

/* <20><>rtklib <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>  */
// 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) /* <20><><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><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;
}