WebSVN – libNMEA – Blame – /trunk/src/nmea.c

Rev	Author	Line No.	Line
1	mjames	1	/*
		2	* nmea.c
		3	*
		4	* Created on: 6 Sep 2020
		5	* Author: mike
		6	*/
		7
		8	#include <math.h>
		9	#include <stdint.h>
		10	#include <stdlib.h>
		11	#include <memory.h>
		12
		13	#include "libNMEA/nmea.h"
		14
		15	#include "libSerial/serial.h"
		16
		17	char linebuff[80];
		18	unsigned linePos = 0;
		19
2	mjames	20	nmeaCallback rmcCallback = NULL;
		21
1	mjames	22	typedef enum
		23	{
3	mjames	24	SEARCH,
		25	READING
1	mjames	26	} NmeaState_t;
		27
		28	NmeaState_t lineState = SEARCH;
		29
		30	static bool
3	mjames	31	decodePacket(char linebuff, int linePos, Location loc);
1	mjames	32
3	mjames	33	bool updateLocation(Location loc, usart_ctl uc)
1	mjames	34	{
3	mjames	35	unsigned chars = SerialCharsReceived(uc);
		36	if (!chars)
		37	return false; // nothing to read, return immediately
1	mjames	38
3	mjames	39	for (int i = 0; i < chars; i++)
		40	{
		41	char c = GetCharSerial(uc);
		42	switch (lineState)
		43	{
		44	case SEARCH:
		45	if (c != '$')
		46	break;
		47	lineState = READING;
		48	linePos = 0;
		49	case READING:
		50	if (c == '\r')
		51	{
		52	// handle the read code
		53	bool success = decodePacket(linebuff, linePos, loc);
		54	lineState = SEARCH;
1	mjames	55
3	mjames	56	linePos = 0;
		57	return success;
		58	}
		59	if (linePos < sizeof(linebuff))
		60	linebuff[linePos++] = c;
		61	else
		62	{
		63	lineState = SEARCH;
		64	// search for the $ in any unread string
		65	int i;
1	mjames	66
3	mjames	67	for (i = 0; i < linePos; i++)
		68	if (linebuff[i] == '$')
		69	{
		70	memcpy(linebuff, linebuff + i, linePos - i);
		71	linePos = 1;
		72	linebuff[linePos++] = c;
		73	lineState = READING;
		74	}
1	mjames	75
3	mjames	76	if (lineState == SEARCH)
		77	linePos = 0;
		78	}
		79	break;
		80	}
1	mjames	81	}
3	mjames	82	return false;
1	mjames	83	}
		84
		85	static int8_t
3	mjames	86	decodeDec(char c)
1	mjames	87	{
3	mjames	88	if (c > '9' && c < '0')
		89	return -1;
		90	return c - '0';
1	mjames	91	}
		92
		93	static uint8_t
3	mjames	94	decodeHex(char c)
1	mjames	95	{
3	mjames	96	int8_t v = decodeDec(c);
		97	if (v >= 0)
		98	return v;
		99	c = tolower(c);
		100	if (c >= 'a' && c <= 'f')
		101	return c - 'a' + 10;
		102	return 0;
1	mjames	103	}
		104
		105	// lat/long decoder
		106	static float
3	mjames	107	decodeLL(char *ptr, int msDigitWeight)
1	mjames	108	{
3	mjames	109	float digitWeight = msDigitWeight;
		110	float result = 0;
		111	int i = 0;
		112	while (1)
1	mjames	113	{
3	mjames	114	char c = ptr[i++];
		115	if (c == '.')
		116	continue;
		117	int8_t v = decodeDec(c);
		118	if (v > 0)
		119	{
		120	result += digitWeight * v;
		121	if (fabs(digitWeight - 1) < 0.01)
		122	digitWeight = 1 / 6.0;
		123	else
		124	digitWeight = digitWeight / 10;
1	mjames	125
3	mjames	126	continue;
		127	}
		128	break;
1	mjames	129	}
		130
3	mjames	131	return result;
1	mjames	132	}
		133
		134	static float
3	mjames	135	decodeFP(char *ptr)
1	mjames	136	{
3	mjames	137	return strtof(ptr, NULL);
1	mjames	138	}
		139
		140	static int
3	mjames	141	decodeDecimal(char *ptr)
1	mjames	142	{
3	mjames	143	int i;
		144	int res = 0;
		145	int const width = 2;
		146	for (i = 0; i < width; i++)
		147	{
		148	int8_t v = decodeDec(*ptr++);
		149	if (v < 0)
		150	return 0;
		151	res *= 10;
		152	res += v;
		153	}
		154	return res;
1	mjames	155	}
		156
3	mjames	157	bool decodePacket(char linebuff, int linePos, Location loc)
1	mjames	158	{
2	mjames	159
3	mjames	160	uint8_t checksum = 0;
		161	for (int i = 1; i < linePos - 3; i++)
		162	checksum ^= linebuff[i];
		163	uint8_t givenSum = (decodeHex(linebuff[linePos - 2]) << 4) + decodeHex(linebuff[linePos - 1]);
		164	if (givenSum != checksum)
		165	return false;
1	mjames	166
3	mjames	167	char *fieldPos[20];
		168	int fieldCnt = 0;
		169	// split fields
		170	for (int i = 6; i < linePos - 3; i++)
1	mjames	171	{
3	mjames	172	if (linebuff[i] == ',')
		173	{
		174	fieldPos[fieldCnt++] = linebuff + i + 1;
		175	}
1	mjames	176	}
		177
3	mjames	178	// decode RMC
		179	if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C')
1	mjames	180
3	mjames	181	{
1	mjames	182
3	mjames	183	// decode the fields
		184	loc->valid = *fieldPos[1];
		185	if (loc->valid == 'A')
		186	{
		187	memcpy(loc->time, fieldPos[0], 6);
		188	loc->lat = decodeLL(fieldPos[2], 10);
		189	loc->ns = *fieldPos[3];
		190	loc->lon = decodeLL(fieldPos[4], 100);
		191	loc->ew = *fieldPos[5];
		192	loc->speed = decodeFP(fieldPos[6]);
		193	loc->heading = decodeFP(fieldPos[7]);
		194	memcpy(loc->date, fieldPos[1], 6);
2	mjames	195
3	mjames	196	loc->tv.tm_sec = decodeDecimal(&loc->time[4]);
		197	loc->tv.tm_min = decodeDecimal(&loc->time[2]);
		198	loc->tv.tm_hour = decodeDecimal(&loc->time[0]);
2	mjames	199
3	mjames	200	loc->tv.tm_mday = decodeDecimal(&loc->date[0]);
		201	loc->tv.tm_mon = decodeDecimal(&loc->date[2]) - 1;
		202	loc->tv.tm_year = decodeDecimal(&loc->date[4]) + 100; //
1	mjames	203
3	mjames	204	loc->tv.tm_isdst = 0;
		205	loc->utc = mktime(&loc->tv);
1	mjames	206
3	mjames	207	loc->good = true;
		208	}
		209	if (rmcCallback)
		210	{
		211	linebuff[linePos++] = '\r';
		212	linebuff[linePos++] = '\n';
		213	linebuff[linePos++] = 0;
		214	rmcCallback((uint8_t *)linebuff, linePos);
		215	}
1	mjames	216	}
2	mjames	217
3	mjames	218	return true;
1	mjames	219	}
2	mjames	220
3	mjames	221	void setRmcCallback(nmeaCallback callback)
2	mjames	222	{
3	mjames	223	rmcCallback = callback;
2	mjames	224	}

Subversion Repositories libNMEA

(root)/trunk/src/nmea.c – Rev 3