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

Subversion Repositories libNMEA

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