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

Rev	Author	Line No.	Line
2	mjames	1	/*
		2	* nmea.c
		3	*
		4	* Created on: 6 Sep 2020
		5	* Author: mike
		6	*/
		7
4	mjames	8	#include <math.h>
		9	#include <stdint.h>
		10	#include <stdlib.h>
		11
2	mjames	12	#include "nmea.h"
		13
		14	#include "libSerial/serial.h"
		15
		16	char linebuff[80];
		17	unsigned linePos = 0;
		18
		19	typedef enum
		20	{
		21	SEARCH, READING
		22	} NmeaState_t;
		23
		24	NmeaState_t lineState = SEARCH;
		25
9	mjames	26	static bool
2	mjames	27	decodePacket (char linebuff, int linePos, Location loc);
		28
9	mjames	29	bool
2	mjames	30	updateLocation (Location *loc)
		31	{
		32	while (1)
		33	{
		34	if (!SerialCharsReceived (&uc1))
		35	return false; // nothing to read, return immediately
		36
		37	char c = GetCharSerial (&uc1);
		38	switch (lineState)
		39	{
		40	case SEARCH:
		41	if (c == '$')
		42	lineState = READING;
9	mjames	43	linePos = 0;
2	mjames	44	break;
		45	case READING:
		46	if (c == '\r')
		47	{
		48	// handle the read code
9	mjames	49	bool success = decodePacket (linebuff, linePos, loc);
2	mjames	50	lineState = SEARCH;
		51
		52	linePos = 0;
		53	return success;
		54	}
		55	if (linePos < sizeof(linebuff))
		56	linebuff[linePos++] = c;
		57	else
		58	{
		59	lineState = SEARCH;
9	mjames	60	// search for the $ in any unread string
		61	int i;
		62
		63	for (i = 0; i < linePos; i++)
		64	if (linebuff[i] == '$')
		65	{
		66	int n = i+1;
		67	memcpy (linebuff, linebuff + n, linePos - i);
		68	linePos = linePos - i;
		69	linebuff[linePos++] = c;
		70	lineState = READING;
		71	}
		72
		73	if (lineState == SEARCH)
		74	linePos = 0;
2	mjames	75	}
		76	break;
		77	}
		78	}
		79	return 0;
		80	}
		81
4	mjames	82	static uint8_t
2	mjames	83	decodeHex (char c)
		84	{
		85	if (c >= '0' && c <= '9')
		86	return c - '0';
		87	c = tolower (c);
		88	if (c >= 'a' && c <= 'f')
		89	return c - 'a' + 10;
		90	return 0;
		91	}
		92
4	mjames	93	// lat/long decoder
		94	static float
		95	decodeLL (char *ptr, int msDigitWeight)
		96	{
		97	float digitWeight = msDigitWeight;
		98	float result = 0;
		99	int i = 0;
		100	while (1)
		101	{
		102	char c = ptr[i++];
		103	if (c == '.')
		104	continue;
		105	if (c >= '0' && c <= '9')
		106	{
		107	result += digitWeight * (c - '0');
		108	if (fabs (digitWeight - 1) < 0.01)
		109	digitWeight = 1 / 6.0;
		110	else
		111	digitWeight = digitWeight / 10;
		112
		113	continue;
		114	}
		115	break;
		116	}
		117
		118	return result;
		119
		120	}
		121
9	mjames	122	static float
		123	decodeFP (char *ptr)
4	mjames	124	{
9	mjames	125	return strtof (ptr, NULL);
4	mjames	126	}
		127
9	mjames	128	bool
2	mjames	129	decodePacket (char linebuff, int linePos, Location loc)
		130	{
5	mjames	131	uint8_t checksum = 0;
2	mjames	132	for (int i = 0; i < linePos - 3; i++)
		133	checksum ^= linebuff[i];
		134	uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
		135	+ decodeHex (linebuff[linePos - 1]);
		136	if (givenSum != checksum)
9	mjames	137	return false;
2	mjames	138
		139	char *fieldPos[20];
		140	int fieldCnt = 0;
		141	// split fields
4	mjames	142	for (int i = 5; i < linePos - 3; i++)
2	mjames	143	{
		144	if (linebuff[i] == ',')
		145	{
		146	fieldPos[fieldCnt++] = linebuff + i + 1;
		147	}
4	mjames	148
2	mjames	149	}
		150
9	mjames	151	// decode RMC
4	mjames	152	if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
2	mjames	153
		154	{
3	mjames	155	// decode the fields
9	mjames	156	memcpy (loc->time, fieldPos[0], 6);
4	mjames	157	loc->valid = *fieldPos[1];
		158	loc->lat = decodeLL (fieldPos[2], 10);
		159	loc->ns = *fieldPos[3];
		160	loc->lon = decodeLL (fieldPos[4], 100);
		161	loc->ew = *fieldPos[5];
9	mjames	162	loc->speed = decodeFP (fieldPos[6]);
4	mjames	163	loc->heading = decodeFP (fieldPos[7]);
9	mjames	164	loc->good = true;
4	mjames	165	}
9	mjames	166	return true;
2	mjames	167	}

Subversion Repositories dashGPS

(root)/trunk/src/nmea.c @ 10 – Rev