stinmea.cpp

#include "stinmea.h"

//TODO: add url to original project, original copyright, LPGL license

// Allow debugging/regression testing under normal g++ environment.
#ifdef MICRONMEA_DEBUG
#include <stdlib.h>
#include <iostream>
using namespace std;
#endif


static long exp10(uint8_t b)
{
	long r = 1;
	while (b--)
		r *= 10;
	return r;
}


static char toHex(uint8_t nibble)
{
	if (nibble >= 10)
		return nibble + 'A' - 10;
	else
		return nibble + '0';

}


const char* STINMEA::skipField(const char* s)
{
	if (s == NULL)
		return NULL;

	while (!isEndOfFields(*s)) {
		if (*s == ',') {
			// Check next character
			if (isEndOfFields(*++s))
				break;
			else
				return s;
		}
		++s;
	}
	return NULL; // End of string or valid sentence
}


unsigned int STINMEA::parseUnsignedInt(const char *s, uint8_t len)
{
	int r = 0;
	while (len--)
		r = 10 * r + *s++ - '0';
	return r;
}


long STINMEA::parseFloat(const char* s, uint8_t log10Multiplier, const char** eptr)
{
	int8_t neg = 1;
	long r = 0;
	while (isspace(*s))
		++s;
	if (*s == '-') {
		neg = -1;
		++s;
	}
	else if (*s == '+')
		++s;

	while (isdigit(*s))
		r = 10*r + *s++ - '0';
	r *= exp10(log10Multiplier);

	if (*s == '.') {
		++s;
		long frac = 0;
		while (isdigit(*s) && log10Multiplier) {
			frac = 10 * frac + *s++ -'0';
			--log10Multiplier;
		}
		frac *= exp10(log10Multiplier);
		r += frac;
	}
	r *= neg; // Include effect of any minus sign

	if (eptr)
		*eptr = skipField(s);

	return r;
}


long STINMEA::parseDegreeMinute(const char* s, uint8_t degWidth,
								  const char **eptr)
{
	if (*s == ',') {
		if (eptr)
			*eptr = skipField(s);
		return 0;
	}
	long r = parseUnsignedInt(s, degWidth) * 10000000L;
	s += degWidth;
	r += parseFloat(s, 7, eptr) / 60;
	return r;
}


const char* STINMEA::parseField(const char* s, char *result, int len)
{
	if (s == NULL)
		return NULL;

	int i = 0;
	while (*s != ',' && !isEndOfFields(*s)) {
		if (result && i++ < len)
			*result++ = *s;
		++s;
	}
	if (result && i < len)
		*result = '\0'; // Terminate unless too long

	if (*s == ',')
		return ++s; // Location of start of next field
	else
		return NULL; // End of string or valid sentence
}


const char* STINMEA::generateChecksum(const char* s, char* checksum)
{
	uint8_t c = 0;
	// Initial $ is omitted from checksum, if present ignore it.
	if (*s == '$')
		++s;

	while (*s != '\0' && *s != '*')
		c ^= *s++;

	if (checksum) {
		checksum[0] = toHex(c / 16);
		checksum[1] = toHex(c % 16);
	}
	return s;
}


bool STINMEA::testChecksum(const char* s)
{
	char checksum[2];
	const char* p = generateChecksum(s, checksum);
	return *p == '*' && p[1] == checksum[0] && p[2] == checksum[1];
}


STINMEA::STINMEA(void) :
	_badChecksumHandler(NULL),
	_unknownSentenceHandler(NULL)
{
	setBuffer(NULL, 0);
	clear();
}


STINMEA::STINMEA(void* buf, uint8_t len) :
	_badChecksumHandler(NULL),
	_unknownSentenceHandler(NULL)
{
	setBuffer(buf, len);
	clear();
}


void STINMEA::setBuffer(void* buf, uint8_t len)
{
	_bufferLen = len;
	_buffer = (char*)buf;
	_ptr = _buffer;
	if (_bufferLen) {
		*_ptr = '\0';
		_buffer[_bufferLen - 1] = '\0';
	}
}


void STINMEA::clear(void)
{
	_type = 255;
	_status = '\0';
	_mode = '\0';
	_eastVelocity = LONG_MIN;
	_northVelocity = LONG_MIN;
	_vertVelocity = LONG_MIN;
	_heading = LONG_MIN; //not a valid heading
	_rtkAge = 65535;
	_rtkRatio = 65535;
	_pitch = 65535;
	_roll = 65535;
	_isValid = false;
	_latitude = 999000000L;
	_longitude = 999000000L;
	_altitude = LONG_MIN;
	//_altitudeValid = false;
	_year = _month = _day = 0;
	_hour = _minute = _second = 99;
	_hundredths = 0;
}


bool STINMEA::process(char c)
{
	if (_buffer == NULL || _bufferLen == 0)
		return false;
	if (c == '\0' || c == '\n' || c == '\r') {
		// Terminate buffer then reset pointer
		*_ptr = '\0';
		_ptr = _buffer;
		_type = 0;

		if (*_buffer == '$' && testChecksum(_buffer)) {
			// Valid message
			const char* data;
			_type=255;
			if (!strncmp(_buffer+1, "PSTI", 4)) {
				_type = 254;
				data = skipField(_buffer+1);

				return processSTI(data);
			}
			else if (_unknownSentenceHandler)
				(*_unknownSentenceHandler)(*this);
		}
		else {
			if (_badChecksumHandler && *_buffer != '\0') // don't send empty buffers as bad checksums!
				(*_badChecksumHandler)(*this);
		}
		// Return true for a complete, non-empty, sentence (even if not a valid one).
		return *_buffer != '\0'; //
	}
	else {
		*_ptr = c;
		if (_ptr < &_buffer[_bufferLen - 1])
			++_ptr;
	}

	return false;
}


const char* STINMEA::parseTime(const char* s)
{
	if (*s == ',')
		return skipField(s);
	_hour = parseUnsignedInt(s, 2);
	_minute = parseUnsignedInt(s + 2, 2);
	_second = parseUnsignedInt(s + 4, 2);
	_hundredths = parseUnsignedInt(s + 7, 2);
	return skipField(s + 9);
}


const char* STINMEA::parseDate(const char* s)
{
	if (*s == ',')
		return skipField(s);
	_day = parseUnsignedInt(s, 2);
	_month = parseUnsignedInt(s + 2, 2);
	_year = parseUnsignedInt(s + 4, 2) + 2000;
	return skipField(s + 6);
}


bool STINMEA::processSTI(const char *s)
{
	// If GxGSV messages are received _talker_ID can be changed after
	// other MicroNMEA sentences. Compatibility modes can set the talker ID
	// to indicate GPS regardless of actual navigation system used.
	
	
	//type field is 3 digits only, supposedly hex but only numerical
	//digits are seen by the SkyTraq
	_type = parseUnsignedInt(s,3);
	s += 4; //eat the digits and comma

	if (_type == 30) {
		//parse time of fix
		s = parseTime(s);
		//status
		_status = s[0];
		s = skipField(s); // eat comma

		//latitude
		_latitude = parseDegreeMinute(s, 2, &s);
		if (*s == ',')
			++s;
		else {
			if (*s == 'S')
				_latitude *= -1;
			s += 2; // Skip N/S and comma
		}

		//longitude
		_longitude = parseDegreeMinute(s, 3, &s);
		if (*s == ',')
			++s;
		else {
			if (*s == 'W')
				_longitude *= -1;
			s += 2; // Skip E/W and comma
		}
		//altitude
		_altitude = parseFloat(s, 3, &s); //mm

		//east vel
		_eastVelocity = parseFloat(s,3, &s); //m/s
		//north vel
		_northVelocity = parseFloat(s,3, &s); //m/s
		//vert vel
		_vertVelocity = parseFloat(s,3, &s); //m/2
		//date of fix
		s = parseDate(s);
		//mode
		_mode = s[0];
		s = skipField(s); // eat comma

		//rtk age
		_rtkAge = parseFloat(s,1, &s); //tenths of a second
		//rtk ratio
		_rtkRatio = parseFloat(s,1,&s); //ratio times 10

	} else if (_type == 36) {
		//time
		s = parseTime(s);
		//date
		s = parseDate(s);
		//heading
		_heading = parseFloat(s,2,&s); //heading times 100
		//pitch
		_pitch = parseFloat(s,2,&s); //degree times 100
		//roll
		_roll = parseFloat(s,2,&s); //degree times 100
		//mode
		//_mode = s[0];
	} 

	// That's all we care about
	return true;
}