made backport of 0.12 gpsnmea plugin compile

7 files changed, 273 insertions, 105 deletions

diff --git a/lib/listplusplus.h b/lib/listplusplus.h
index d71272a2..7e9e99e6 100644
--- a/lib/listplusplus.h
+++ b/lib/listplusplus.h
@@ -60,4 +60,15 @@ bool contains(T iteratable, V value)
 	return (std::find(iteratable.begin(), iteratable.end(), value) != iteratable.end());
 }
 
+template <class T, class V>
+void removeOne(T * iteratable, V value)
+{
+	typename T::iterator itr = std::find(iteratable->begin(), iteratable->end(), value);
+
+	if (itr != iteratable->end())
+	{
+		iteratable->erase(itr);
+	}
+}
+
 #endif // LISTPLUSPLUS_H
diff --git a/plugins/common/abstractio.hpp b/plugins/common/abstractio.hpp
index fc4d0fbc..3e0aa676 100644
--- a/plugins/common/abstractio.hpp
+++ b/plugins/common/abstractio.hpp
@@ -9,6 +9,7 @@ public:
 
 	virtual bool open() = 0;
 	virtual bool close() = 0;
+	virtual bool isOpen() =0;
 	virtual std::string read() = 0;
 	virtual void write(std::string data) = 0;
 
diff --git a/plugins/common/serialport.hpp b/plugins/common/serialport.hpp
index bd367ffc..4fa0418d 100644
--- a/plugins/common/serialport.hpp
+++ b/plugins/common/serialport.hpp
@@ -15,11 +15,14 @@
 
 class SerialPort: public AbstractIo
 {
+private:
+	speed_t speed;
+
 public:
 	SerialPort(std::string _tty)
-		:tty(_tty)
+		:tty(_tty), fd(0)
 	{
-
+		speed = B9600;
 	}
 
 	~SerialPort()
@@ -27,6 +30,32 @@ public:
 		close();
 	}
 
+	int setSpeed(int newspeed)
+	{
+		int ret = 0;
+		switch(newspeed)
+		{
+			case 2400: 
+				speed = B2400;
+				break;
+			case 4800: 
+				speed = B4800;
+				break;
+			case 9600: 
+				speed = B9600;
+				break;
+			case 19200: 
+				speed = B19200;
+				break;
+			case 38400: 
+				speed = B38400;
+				break;
+			default:
+				ret = -EINVAL;
+		}
+		return ret;
+	}
+
 	bool open()
 	{
 		fd = ::open(tty.c_str(), O_RDWR, O_NOCTTY);
@@ -53,8 +82,8 @@ public:
 
 		//oldtio.c_cc[VEOL]     = '\r';
 
-		cfsetispeed(&oldtio, B9600);
-		cfsetospeed(&oldtio, B9600);
+		cfsetispeed(&oldtio, speed);
+		cfsetospeed(&oldtio, speed);
 
 		tcflush(fd, TCIFLUSH);
 		tcsetattr(fd, TCSANOW, &oldtio);
@@ -64,6 +93,11 @@ public:
 		return true;
 	}
 
+	bool isOpen()
+	{
+		return (fd > 0);
+	}
+
 	int fileDescriptor() { return fd; }
 
 	bool close()
diff --git a/plugins/gpsnmea/CMakeLists.txt b/plugins/gpsnmea/CMakeLists.txt
index f4a3b669..fddfe883 100644
--- a/plugins/gpsnmea/CMakeLists.txt
+++ b/plugins/gpsnmea/CMakeLists.txt
@@ -5,10 +5,11 @@ set(gpsnmea_sources gpsnmea.cpp )
 
 include_directories(${CMAKE_SOURCE_DIR}/lib ${include_dirs} ${gio_INCLUDE_DIRS} ${CMAKE_SOURCE_DIR}/plugins/common)
 
-add_library(gpsnmea MODULE ${gpsnmea_sources})
-set_target_properties(gpsnmea PROPERTIES PREFIX "")
-target_link_libraries(gpsnmea amb -L${CMAKE_CURRENT_BINARY_DIR}/lib  amb-plugins-common -L${CMAKE_CURRENT_BINARY_DIR}/plugins/common ${link_libraries} boost_regex ${gio_LIBRARIES})
+add_executable(gpsnmea.so ${gpsnmea_sources})
+#add_library(gpsnmea MODULE ${gpsnmea_sources})
+set_target_properties(gpsnmea.so PROPERTIES PREFIX "")
+target_link_libraries(gpsnmea.so amb -L${CMAKE_CURRENT_BINARY_DIR}/lib  amb-plugins-common -L${CMAKE_CURRENT_BINARY_DIR}/plugins/common ${link_libraries} ${gio_LIBRARIES})
 
-install(TARGETS gpsnmea LIBRARY DESTINATION lib/automotive-message-broker)
+install(TARGETS gpsnmea.so RUNTIME DESTINATION lib/automotive-message-broker)
 
 endif(gpsnmea_plugin)
diff --git a/plugins/gpsnmea/README b/plugins/gpsnmea/README
index 3a129d3f..e538b9c6 100644
--- a/plugins/gpsnmea/README
+++ b/plugins/gpsnmea/README
@@ -12,6 +12,7 @@ To use the Database plugin, add the following to the "sources" array in /etc/amb
 	"name" : "gpsnmea",
 	"path" : "/usr/lib/automotive-message-broker/gpsnmea.so",
 	"device" : "/dev/ttyUSB0",
+	"baudrate" : "4800",
 	"bluetoothAdapter" : "00:00:00:00:00:00"
 }
 
@@ -28,6 +29,11 @@ gps device.  This must be a serial device.  It can also be a bluetooth address.
 
 Default: none
 
+"baudrate"
+Baudrate for serial devices : 2400,4800,9600,19200 or 38400.
+
+Default: 9600
+
 "bluetoothAdapter"
 bluetooth adapter to use.  This is only used if "device" is also a bluetooth device.
 If left blank, the system default adapter will be used.
diff --git a/plugins/gpsnmea/gpsnmea.cpp b/plugins/gpsnmea/gpsnmea.cpp
index b5fa7eb5..75ff3403 100644
--- a/plugins/gpsnmea/gpsnmea.cpp
+++ b/plugins/gpsnmea/gpsnmea.cpp
@@ -20,10 +20,10 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 #include "timestamp.h"
 #include "serialport.hpp"
 #include "bluetooth.hpp"
+#include <listplusplus.h>
 
 #include <iostream>
 #include <boost/assert.hpp>
-#include <boost/regex.hpp>
 #include <boost/algorithm/string.hpp>
 #include <time.h>
 
@@ -44,19 +44,6 @@ inline T2 lexical_cast(const std::string &in) {
 	return out;
 }
 
-std::string gprmcRegEx = "[\\$]?GPRMC,([0-1][0-9]|2[0-3])([0-5][0-9])([0-5][0-9])," /** time hh mm ss **/
-		"([AV])," /** Status A= Active, V= Void **/
-		"([0-8][0-9]|90)([0-5][0-9]\\.[0-9]{4})," /** latitude **/
-		"([NS])," /** lat North or South **/
-		"(180|0[0-9]{2}|1[0-7][0-9])([0-5][0-9]\\.[0-9]{4})," /** longitude **/
-		"([EW])," /** lon E or W **/
-		"([0-9]{3}\\.[0-9])," /** Speed in knots **/
-		"(3[0-5][0-9]|[0-2][0-9]{2}\\.[0-9])," /** Direction **/
-		"(3[0-1]|[1-2][0-9]|0[1-9])(0[1-9]|1[0-2])([0-9]{2})," /** Date stamp **/
-		"(180|[1][0-7][0-9]|[0][0-9]{2}\\.[0-9])," /** Magnetic variation **/
-		"([EW])" /** Magnetic Direction **/
-		"\\*([0-9,A-F]{2})";
-
 class Location
 {
 public:
@@ -131,7 +118,6 @@ private:
 	VehicleProperty::VehicleSpeedType mSpeed;
 	BasicPropertyType<double> mGpsTime;
 
-	boost::regex regularExpression;
 	bool isActive;
 
 	std::string mUuid;
@@ -156,39 +142,36 @@ void Location::parse(string nmea)
 	{
 		parseGpgga(nmea);
 	}
+	else
+	{
+		DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
+	}
 }
 
 void Location::parseGprmc(string gprmc)
 {
 	DebugOut(7)<<"parsing gprmc message"<<endl;
 
-	regularExpression.assign(gprmcRegEx);
-
-	boost::smatch tokens;
+	std::vector<std::string> tokens;
+	boost::split(tokens, gprmc, boost::is_any_of(","));
 
-	if (boost::regex_match (gprmc, tokens, regularExpression) )
+	if(!tokens.size())
 	{
+		return;
+	}
 
+	if(tokens[2] == "A")
+	{
+		isActive = true;
+	}
 
-		if(tokens[4] == "A")
-		{
-			isActive = true;
-		}
-
-		int i=0;
-		for(auto tok : tokens)
-		{
-			DebugOut(0)<<i++<<":"<<tok<<endl;
-
-		}
+	parseTime(tokens[1].substr(0,2),tokens[1].substr(2,2),tokens[1].substr(4,2),tokens[9].substr(0,2),tokens[9].substr(2,2),tokens[9].substr(4,2));
 
-		parseTime(tokens[1],tokens[2],tokens[3],tokens[13],tokens[14],tokens[15]);
+	parseLatitude(tokens[3], "", tokens[4]);
+	parseLongitude(tokens[5], "", tokens[6]);
+	parseSpeed(tokens[7]);
+	parseDirection(tokens[8]);
 
-		parseLatitude(tokens[5], tokens[6], tokens[7]);
-		parseLongitude(tokens[8], tokens[9], tokens[10]);
-		parseSpeed(tokens[11]);
-		parseDirection(tokens[12]);
-	}
 }
 
 void Location::parseGpgga(string gpgga)
@@ -216,22 +199,30 @@ void Location::parseGpgga(string gpgga)
 
 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
 {
-	tm t;
-	t.tm_hour = boost::lexical_cast<int>(h);
-	t.tm_min = boost::lexical_cast<int>(m);
-	t.tm_sec = boost::lexical_cast<int>(s);
-	t.tm_mday = boost::lexical_cast<int>(dd);
-	t.tm_mon = boost::lexical_cast<int>(mm);
-	t.tm_year = boost::lexical_cast<int>(yy) + 100;
+	try
+	{
+		tm t;
+		t.tm_hour = boost::lexical_cast<int>(h);
+		t.tm_min = boost::lexical_cast<int>(m);
+		t.tm_sec = boost::lexical_cast<int>(s);
+		t.tm_mday = boost::lexical_cast<int>(dd);
+		t.tm_mon = boost::lexical_cast<int>(mm);
+		t.tm_year = boost::lexical_cast<int>(yy) + 100;
 
-	time_t time = mktime(&t);
+		time_t time = mktime(&t);
 
-	BasicPropertyType<double> temp(GPSTIME,(double)time);
+		BasicPropertyType<double> temp(GPSTIME,(double)time);
 
-	if(mGpsTime != temp)
+		if(mGpsTime != temp)
+		{
+			mGpsTime = temp;
+			if(routingEngine)
+				routingEngine->updateProperty(&mGpsTime, mUuid);
+		}
+	}
+	catch(...)
 	{
-		mGpsTime = temp;
-		routingEngine->updateProperty(&mGpsTime, mUuid);
+		DebugOut(5)<<"Failed to parse time "<<endl;
 	}
 }
 
@@ -252,13 +243,15 @@ void Location::parseLatitude(string d, string m, string ns)
 
 		if(mLatitude != temp)
 		{
-			mLatitude = temp;\
-			routingEngine->updateProperty(&mLatitude, mUuid);
+			mLatitude = temp;
+
+			if(routingEngine)
+				routingEngine->updateProperty(&mLatitude, mUuid);
 		}
 	}
 	catch(...)
 	{
-		DebugOut(DebugOut::Warning)<<"Failed to parse latitude"<<endl;
+		DebugOut(5)<<"Failed to parse latitude"<<endl;
 	}
 }
 
@@ -278,39 +271,57 @@ void Location::parseLongitude(string d, string m, string ew)
 
 		if(mLongitude != temp)
 		{
-			mLongitude = temp;\
-			routingEngine->updateProperty(&mLongitude, mUuid);
+			mLongitude = temp;
+
+			if(routingEngine)
+				routingEngine->updateProperty(&mLongitude, mUuid);
 		}
 	}
 	catch(...)
 	{
-		DebugOut(DebugOut::Warning)<<"failed to parse longitude"<<endl;
+		DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
 	}
 }
 
 void Location::parseSpeed(string spd)
 {
-	double s = boost::lexical_cast<double>(spd);
+	try
+	{
+		double s = boost::lexical_cast<double>(spd);
 
-	///to kph:
-	s *= 1.852;
-	VehicleProperty::VehicleSpeedType temp(s);
-	if(mSpeed != temp)
+		///to kph:
+		s *= 1.852;
+		VehicleProperty::VehicleSpeedType temp(s);
+		if(mSpeed != temp)
+		{
+			mSpeed = temp;
+
+			if(routingEngine)
+				routingEngine->updateProperty(&mSpeed, mUuid);
+		}
+	}
+	catch(...)
 	{
-		mSpeed = temp;
-		routingEngine->updateProperty(&mSpeed, mUuid);
+		DebugOut(5)<<"failed to parse speed"<<endl;
 	}
 }
 
 void Location::parseDirection(string dir)
 {
-	uint16_t d = boost::lexical_cast<double>(dir);
+	try {
+		uint16_t d = boost::lexical_cast<double>(dir);
 
-	VehicleProperty::DirectionType temp(d);
-	if(mDirection != temp)
+		VehicleProperty::DirectionType temp(d);
+		if(mDirection != temp)
+		{
+			mDirection = temp;
+			if(routingEngine)
+				routingEngine->updateProperty(&mDirection, mUuid);
+		}
+	}
+	catch(...)
 	{
-		mDirection = temp;
-		routingEngine->updateProperty(&mDirection, mUuid);
+		DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
 	}
 }
 
@@ -326,14 +337,16 @@ void Location::parseAltitude(string alt)
 		if(mAltitude != temp)
 		{
 			mAltitude = temp;
-			routingEngine->updateProperty(&mAltitude, mUuid);
+
+			if(routingEngine)
+				routingEngine->updateProperty(&mAltitude, mUuid);
 		}
 
 		mAltitude = VehicleProperty::AltitudeType(a);
 	}
 	catch(...)
 	{
-		DebugOut(DebugOut::Warning)<<"failed to parse altitude"<<endl;
+		DebugOut(5)<<"failed to parse altitude"<<endl;
 	}
 }
 
@@ -375,8 +388,9 @@ extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<str
 }
 
 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
-	:AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
+	:AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr)
 {
+	int baudrate = 0;
 	location =new Location(re, mUuid);
 
 	VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
@@ -393,24 +407,16 @@ GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> conf
 
 	if(config.find("test") != config.end())
 	{
-		Location location(routingEngine, mUuid);
-		location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
-
-		DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
-
-		g_assert(location.latitude().toString() == "-23.86600833");
-		g_assert(location.gpsTime().toString() == "1050585131");
-
-		location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
-
-		DebugOut()<<"alt: "<<location.altitude().toString()<<endl;
-		DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
-		g_assert(location.altitude().toString() == "545.4");
-		g_assert(location.latitude().toString() == "48.1173");
+		test();
 	}
 
 	std::string btaddapter = config["bluetoothAdapter"];
 
+	if(config.find("baudrate")!= config.end())
+	{
+		baudrate = boost::lexical_cast<int>( config["baudrate"] );
+	}
+
 	if(config.find("device")!= config.end())
 	{
 		std::string dev = config["device"];
@@ -422,6 +428,12 @@ GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> conf
 
 		device = new SerialPort(dev);
 
+		if(baudrate!=0)
+		{
+			if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
+				DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
+		}
+
 		if(!device->open())
 		{
 			DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
@@ -436,13 +448,12 @@ GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> conf
 		g_io_channel_set_close_on_unref(chan, true);
 		g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
 	}
-
-	re->setSupported(supported(), this);
 }
 
 GpsNmeaSource::~GpsNmeaSource()
 {
-	device->close();
+	if(device && device->isOpen())
+		device->close();
 }
 
 const string GpsNmeaSource::uuid()
@@ -502,35 +513,121 @@ void GpsNmeaSource::canHasData()
 {
 	std::string data = device->read();
 
+	tryParse(data);
+}
+
+void GpsNmeaSource::test()
+{
+	Location location(nullptr, "");
+	location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
+
+	DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
+
+	g_assert(location.latitude().toString() == "-23.86600833");
+	g_assert(location.gpsTime().toString() == "1050585131");
+
+	location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
+
+	DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
+	DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
+	g_assert(location.altitude().toString() == "545.4");
+	g_assert(location.latitude().toString() == "48.1173");
+
+	location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
+	DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
+	DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
+
+	//Test incomplete message:
+	location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
+	DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
+	DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
+
+	std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
+
+	g_assert(checksum(testChecksuming));
+
+	std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
+	std::string multimessage2 = "57383,W,";
+	std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
+	std::string multimessage4 = "GPGSA,A,";
+	std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
+	std::string multimessage6 = ",7.53,1.00*";
+	std::string multimessage7 = "0E";
+
+	bool multimessageParse = false;
+
+	multimessageParse |= tryParse(multimessage1);
+	multimessageParse |= tryParse(multimessage2);
+	multimessageParse |= tryParse(multimessage3);
+	multimessageParse |= tryParse(multimessage4);
+	multimessageParse |= tryParse(multimessage5);
+	multimessageParse |= tryParse(multimessage6);
+	multimessageParse |= tryParse(multimessage7);
+
+	g_assert(multimessageParse);
+
+	//test false message:
+
+	g_assert(!checksum("GPRMC,172758.296,V"));
+}
+
+bool GpsNmeaSource::tryParse(string data)
+{
 	std::vector<std::string> lines;
 
-	boost::split(lines,data,boost::is_any_of("$"));
+	boost::split(lines, data, boost::is_any_of("$"));
+
+	bool weFoundAMessage = false;
 
-	for(int i = 0; i < lines.size(); i++)
+	for(auto line : lines)
 	{
-		if(checksum(lines[i]))
+		if(checksum(line))
 		{
-			buffer = lines[i];
+			buffer = line;
 		}
 		else
 		{
-			buffer += lines[i];
+			buffer += line;
+		}
+
+		std::string::size_type pos = buffer.find('G');
+
+		if(pos != std::string::npos && pos != 0)
+		{
+			///Throw the incomplete stuff away.  if it doesn't begin with "G" it'll never be complete
+			buffer = buffer.substr(pos);
 		}
 
 		if(checksum(buffer))
 		{
 			/// we have a complete message.  parse it!
+			DebugOut(7)<<"Complete message: "<<buffer<<endl;
 			location->parse(buffer);
+			weFoundAMessage = true;
+			buffer = "";
+		}
+		else
+		{
+			if(pos == 0 )
+			{
+				uint cs = buffer.find('*');
+				if (cs != std::string::npos && cs != buffer.length()-1)
+				{
+					///This means we have a false flag somewhere.
+					buffer = buffer.substr(cs+(buffer.length() - cs));
+				}
+			}
 		}
 
 		DebugOut(7)<<"buffer: "<<buffer<<endl;
-
 	}
+
+	return weFoundAMessage;
 }
 
 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
 {
-	mRequests.remove(property);
+	removeOne(&mRequests,property);
 }
 
 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
@@ -548,7 +645,7 @@ void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone:
 
 bool GpsNmeaSource::checksum(std::string sentence)
 {
-	if(sentence.empty() || sentence.length() < 4)
+	if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
 	{
 		return false;
 	}
@@ -563,7 +660,7 @@ bool GpsNmeaSource::checksum(std::string sentence)
 			checksum ^= i;
 	}
 
-	std::string sentenceCheckStr = sentence.substr(sentence.length()-4,2);
+	std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
 
 	try
 	{
@@ -576,4 +673,16 @@ bool GpsNmeaSource::checksum(std::string sentence)
 	{
 		return false;
 	}
+
+	return false;
+}
+
+
+int main(int argc, char** argv)
+{
+	DebugOut::setDebugThreshhold(7);
+	GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());
+	plugin.test();
+
+	return 1;
 }
diff --git a/plugins/gpsnmea/gpsnmea.h b/plugins/gpsnmea/gpsnmea.h
index 7fd6f971..0304f4e9 100644
--- a/plugins/gpsnmea/gpsnmea.h
+++ b/plugins/gpsnmea/gpsnmea.h
@@ -45,7 +45,7 @@ public:
 
 	int supportedOperations();
 	
-	void supportedChanged(PropertyList) {}
+	void supportedChanged(PropertyList ) {}
 	
 	PropertyInfo getPropertyInfo(VehicleProperty::Property property)
 	{
@@ -56,9 +56,15 @@ public:
 	}
 
 	void canHasData();
-	
+
+	void test();
+
 private:
 
+
+
+	bool tryParse(std::string data);
+
 	void addPropertySupport(VehicleProperty::Property property, Zone::Type zone);
 
 	bool checksum(string sentence);