IMU_DUAL/nmea/nmea_zjut.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;
}