2 Copyright (C) 2012 Intel Corporation
4 This library is free software; you can redistribute it and/or
5 modify it under the terms of the GNU Lesser General Public
6 License as published by the Free Software Foundation; either
7 version 2.1 of the License, or (at your option) any later version.
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 Lesser General Public License for more details.
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 #include "timestamp.h"
21 #include "serialport.hpp"
22 #include "bluetooth.hpp"
25 #include <boost/assert.hpp>
26 #include <boost/regex.hpp>
27 #include <boost/algorithm/string.hpp>
34 #include "abstractpropertytype.h"
36 #define GPSTIME "GpsTime"
38 std::string gprmcRegEx = "[\\$]?GPRMC,([0-1][0-9]|2[0-3])([0-5][0-9])([0-5][0-9])," /** time hh mm ss **/
39 "([AV])," /** Status A= Active, V= Void **/
40 "([0-8][0-9]|90)([0-5][0-9]\\.[0-9]{4})," /** latitude **/
41 "([NS])," /** lat North or South **/
42 "(180|0[0-9]{2}|1[0-7][0-9])([0-5][0-9]\\.[0-9]{4})," /** longitude **/
43 "([EW])," /** lon E or W **/
44 "([0-9]{3}\\.[0-9])," /** Speed in knots **/
45 "(3[0-5][0-9]|[0-2][0-9]{2}\\.[0-9])," /** Direction **/
46 "(3[0-1]|[1-2][0-9]|0[1-9])(0[1-9]|1[0-2])([0-9]{2}),"
47 "(180|[1][0-7][0-9]|[0][0-9]{2}\\.[0-9])," /** Magnetic variation **/
48 "([EW])" /** Magnetic Direction **/
54 Location(AbstractRoutingEngine* re, std::string uuid);
56 void parse(std::string gprmc);
58 VehicleProperty::LatitudeType latitude()
63 VehicleProperty::LongitudeType longitude()
68 VehicleProperty::AltitudeType altitude()
73 VehicleProperty::DirectionType direction()
78 VehicleProperty::VehicleSpeedType speed()
83 BasicPropertyType<double> gpsTime()
90 void parseGprmc(string gprmc);
91 void parseGpgga(string gpgga);
93 void parseTime(std::string h, std::string m, std::string s);
94 void parseLatitude(std::string d, std::string m, std::string ns);
95 void parseLongitude(std::string d, string m, string ew);
96 void parseSpeed(std::string spd);
97 void parseDirection(std::string dir);
98 void parseAltitude(std::string alt);
100 double degsToDecimal(double degs);
104 VehicleProperty::LatitudeType mLatitude;
105 VehicleProperty::LongitudeType mLongitude;
106 VehicleProperty::AltitudeType mAltitude;
107 VehicleProperty::DirectionType mDirection;
108 VehicleProperty::VehicleSpeedType mSpeed;
109 BasicPropertyType<double> mGpsTime;
111 boost::regex regularExpression;
116 AbstractRoutingEngine* routingEngine;
120 Location::Location(AbstractRoutingEngine* re, std::string uuid)
121 :mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
126 void Location::parse(string nmea)
128 if(boost::algorithm::starts_with(nmea,"GPRMC"))
132 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
138 void Location::parseGprmc(string gprmc)
140 regularExpression.assign(gprmcRegEx);
142 boost::smatch tokens;
144 if (boost::regex_match (gprmc, tokens, regularExpression) )
146 parseTime(tokens[1],tokens[2],tokens[3]);
153 parseLatitude(tokens[5], tokens[6], tokens[7]);
154 parseLongitude(tokens[8], tokens[9], tokens[10]);
155 parseSpeed(tokens[11]);
156 parseDirection(tokens[12]);
160 void Location::parseGpgga(string gpgga)
163 std::vector<std::string> tokens;
164 boost::split(tokens, gpgga, boost::is_any_of(","));
166 if(tokens.size() != 15)
168 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
172 parseTime(tokens[1].substr(0,2),tokens[1].substr(2,2),tokens[1].substr(4,2));
173 parseLatitude(tokens[2],"",tokens[3]);
174 parseLongitude(tokens[4],"",tokens[5]);
179 else isActive = false;
181 parseAltitude(tokens[9]);
184 void Location::parseTime(string h, string m, string s)
187 t.tm_hour = boost::lexical_cast<int>(h);
188 t.tm_min = boost::lexical_cast<int>(m);
189 t.tm_sec = boost::lexical_cast<int>(s);
191 time_t time = mktime(&t);
193 BasicPropertyType<double> temp(GPSTIME,(double)time);
198 routingEngine->updateProperty(GPSTIME, &mGpsTime, mUuid);
202 void Location::parseLatitude(string d, string m, string ns)
204 double degs = boost::lexical_cast<double>(d + m);
205 double dec = degsToDecimal(degs);
210 VehicleProperty::LatitudeType temp(dec);
212 if(mLatitude != temp)
215 routingEngine->updateProperty(VehicleProperty::Latitude, &mLatitude, mUuid);
219 void Location::parseLongitude(string d, string m, string ew)
221 double degs = boost::lexical_cast<double>(d + m);
222 double dec = degsToDecimal(degs);
227 VehicleProperty::LongitudeType temp(dec);
229 if(mLongitude != temp)
232 routingEngine->updateProperty(VehicleProperty::Longitude, &mLongitude, mUuid);
236 void Location::parseSpeed(string spd)
238 double s = boost::lexical_cast<double>(spd);
242 VehicleProperty::VehicleSpeedType temp(s);
246 routingEngine->updateProperty(VehicleProperty::VehicleSpeed, &mSpeed, mUuid);
250 void Location::parseDirection(string dir)
252 uint16_t d = boost::lexical_cast<double>(dir);
254 VehicleProperty::DirectionType temp(d);
255 if(mDirection != temp)
258 routingEngine->updateProperty(VehicleProperty::Direction, &mDirection, mUuid);
262 void Location::parseAltitude(string alt)
264 double a = boost::lexical_cast<double>(alt);
266 VehicleProperty::AltitudeType temp(a);
267 if(mAltitude != temp)
270 routingEngine->updateProperty(VehicleProperty::Altitude, &mAltitude, mUuid);
273 mAltitude = VehicleProperty::AltitudeType(a);
276 double Location::degsToDecimal(double degs)
279 double min = 100.0 * modf(degs / 100.0, °);
280 return deg + (min / 60.0);
283 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
285 // DebugOut(5) << "Polling..." << condition << endl;
287 if(condition & G_IO_ERR)
289 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
292 if (condition & G_IO_HUP)
294 //Hang up. Returning false closes out the GIOChannel.
295 //printf("Callback on G_IO_HUP\n");
296 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
300 GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
307 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
309 return new GpsNmeaSource(routingengine, config);
313 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
314 :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
316 location =new Location(re, mUuid);
318 VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
320 addPropertySupport(VehicleProperty::Latitude, Zone::None);
321 addPropertySupport(VehicleProperty::Longitude, Zone::None);
322 addPropertySupport(VehicleProperty::Altitude, Zone::None);
323 addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
324 addPropertySupport(VehicleProperty::Direction, Zone::None);
325 addPropertySupport(GPSTIME, Zone::None);
330 if(config.find("test") != config.end())
332 Location location(routingEngine, mUuid);
333 location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
335 DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
337 g_assert(location.latitude().toString() == "-23.86600833");
339 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
341 DebugOut()<<"alt: "<<location.altitude().toString()<<endl;
342 DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
343 g_assert(location.altitude().toString() == "545.4");
344 g_assert(location.latitude().toString() == "48.1173");
347 std::string btaddapter = config["bluetoothAdapter"];
349 if(config.find("device")!= config.end())
351 std::string dev = config["device"];
352 if(dev.find(":") != string::npos)
355 dev = bt.getDeviceForAddress(dev, btaddapter);
358 device = new SerialPort(dev);
362 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
367 DebugOut()<<"read from device: "<<device->read()<<endl;
369 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
370 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
371 g_io_channel_set_close_on_unref(chan, true);
372 g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
375 re->setSupported(supported(), this);
378 GpsNmeaSource::~GpsNmeaSource()
383 const string GpsNmeaSource::uuid()
389 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
391 DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
395 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
400 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
405 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
407 mRequests.push_back(property);
410 PropertyList GpsNmeaSource::supported()
415 int GpsNmeaSource::supportedOperations()
420 void GpsNmeaSource::canHasData()
422 std::string data = device->read();
424 std::vector<std::string> lines;
426 boost::split(lines,data,boost::is_any_of("$"));
428 for(int i = 0; i < lines.size(); i++)
430 location->parse(lines[i]);
434 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
436 mRequests.remove(property);
439 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
441 mSupported.push_back(property);
443 std::list<Zone::Type> zones;
445 zones.push_back(zone);
447 PropertyInfo info(0, zones);
449 propertyInfoMap[property] = info;