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"
23 #include <listplusplus.h>
26 #include <boost/assert.hpp>
27 #include <boost/algorithm/string.hpp>
34 #include "abstractpropertytype.h"
36 #define GPSTIME "GpsTime"
39 inline T2 lexical_cast(const std::string &in) {
50 Location(AbstractRoutingEngine* re, std::string uuid);
52 void parse(std::string gprmc);
54 VehicleProperty::LatitudeType latitude()
59 VehicleProperty::LongitudeType longitude()
64 VehicleProperty::AltitudeType altitude()
69 VehicleProperty::DirectionType direction()
74 VehicleProperty::VehicleSpeedType speed()
79 BasicPropertyType<double> gpsTime()
84 std::list<AbstractPropertyType*> fix()
86 std::list<AbstractPropertyType*> l;
88 l.push_back(&mLatitude);
89 l.push_back(&mLongitude);
90 l.push_back(&mAltitude);
91 l.push_back(&mDirection);
93 l.push_back(&mGpsTime);
100 void parseGprmc(string gprmc);
101 void parseGpgga(string gpgga);
103 void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
104 void parseLatitude(std::string d, std::string m, std::string ns);
105 void parseLongitude(std::string d, string m, string ew);
106 void parseSpeed(std::string spd);
107 void parseDirection(std::string dir);
108 void parseAltitude(std::string alt);
110 double degsToDecimal(double degs);
114 VehicleProperty::LatitudeType mLatitude;
115 VehicleProperty::LongitudeType mLongitude;
116 VehicleProperty::AltitudeType mAltitude;
117 VehicleProperty::DirectionType mDirection;
118 VehicleProperty::VehicleSpeedType mSpeed;
119 BasicPropertyType<double> mGpsTime;
125 AbstractRoutingEngine* routingEngine;
129 Location::Location(AbstractRoutingEngine* re, std::string uuid)
130 :mLatitude(0), mLongitude(0), mAltitude(0), mDirection(0), mSpeed(0), mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
135 void Location::parse(string nmea)
137 if(boost::algorithm::starts_with(nmea,"GPRMC"))
141 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
147 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
151 void Location::parseGprmc(string gprmc)
153 DebugOut(7)<<"parsing gprmc message"<<endl;
155 std::vector<std::string> tokens;
156 boost::split(tokens, gprmc, boost::is_any_of(","));
168 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));
170 parseLatitude(tokens[3], "", tokens[4]);
171 parseLongitude(tokens[5], "", tokens[6]);
172 parseSpeed(tokens[7]);
173 parseDirection(tokens[8]);
177 void Location::parseGpgga(string gpgga)
180 std::vector<std::string> tokens;
181 boost::split(tokens, gpgga, boost::is_any_of(","));
183 if(tokens.size() != 15)
185 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
189 parseLatitude(tokens[2],"",tokens[3]);
190 parseLongitude(tokens[4],"",tokens[5]);
195 else isActive = false;
197 parseAltitude(tokens[9]);
200 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
205 t.tm_hour = boost::lexical_cast<int>(h);
206 t.tm_min = boost::lexical_cast<int>(m);
207 t.tm_sec = boost::lexical_cast<int>(s);
208 t.tm_mday = boost::lexical_cast<int>(dd);
209 t.tm_mon = boost::lexical_cast<int>(mm);
210 t.tm_year = boost::lexical_cast<int>(yy) + 100;
212 time_t time = mktime(&t);
214 BasicPropertyType<double> temp(GPSTIME,(double)time);
219 routingEngine->updateProperty(&mGpsTime, mUuid);
224 DebugOut(DebugOut::Warning)<<"Failed to parse time "<<endl;
228 void Location::parseLatitude(string d, string m, string ns)
235 double degs = boost::lexical_cast<double>(d + m);
236 double dec = degsToDecimal(degs);
241 VehicleProperty::LatitudeType temp(dec);
243 if(mLatitude != temp)
246 routingEngine->updateProperty(&mLatitude, mUuid);
251 DebugOut(DebugOut::Warning)<<"Failed to parse latitude"<<endl;
255 void Location::parseLongitude(string d, string m, string ew)
259 if(d.empty()) return;
261 double degs = boost::lexical_cast<double>(d + m);
262 double dec = degsToDecimal(degs);
267 VehicleProperty::LongitudeType temp(dec);
269 if(mLongitude != temp)
272 routingEngine->updateProperty(&mLongitude, mUuid);
277 DebugOut(DebugOut::Warning)<<"failed to parse longitude"<<endl;
281 void Location::parseSpeed(string spd)
285 double s = boost::lexical_cast<double>(spd);
289 VehicleProperty::VehicleSpeedType temp(s);
293 routingEngine->updateProperty(&mSpeed, mUuid);
298 DebugOut(DebugOut::Warning)<<"failed to parse speed"<<endl;
302 void Location::parseDirection(string dir)
305 uint16_t d = boost::lexical_cast<double>(dir);
307 VehicleProperty::DirectionType temp(d);
308 if(mDirection != temp)
311 routingEngine->updateProperty(&mDirection, mUuid);
316 DebugOut(DebugOut::Warning)<<"Failed to parse direction: "<<dir<<endl;
320 void Location::parseAltitude(string alt)
324 if(alt.empty()) return;
326 double a = boost::lexical_cast<double>(alt);
328 VehicleProperty::AltitudeType temp(a);
329 if(mAltitude != temp)
332 routingEngine->updateProperty(&mAltitude, mUuid);
335 mAltitude = VehicleProperty::AltitudeType(a);
339 DebugOut(DebugOut::Warning)<<"failed to parse altitude"<<endl;
343 double Location::degsToDecimal(double degs)
346 double min = 100.0 * modf(degs / 100.0, °);
347 return deg + (min / 60.0);
350 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
352 // DebugOut(5) << "Polling..." << condition << endl;
354 if(condition & G_IO_ERR)
356 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
359 if (condition & G_IO_HUP)
361 //Hang up. Returning false closes out the GIOChannel.
362 //printf("Callback on G_IO_HUP\n");
363 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
367 GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
374 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
376 return new GpsNmeaSource(routingengine, config);
380 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
381 :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
384 location =new Location(re, mUuid);
386 VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
388 addPropertySupport(VehicleProperty::Latitude, Zone::None);
389 addPropertySupport(VehicleProperty::Longitude, Zone::None);
390 addPropertySupport(VehicleProperty::Altitude, Zone::None);
391 addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
392 addPropertySupport(VehicleProperty::Direction, Zone::None);
393 addPropertySupport(GPSTIME, Zone::None);
398 if(config.find("test") != config.end())
400 Location location(routingEngine, mUuid);
401 location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
403 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
405 g_assert(location.latitude().toString() == "-23.86600833");
406 g_assert(location.gpsTime().toString() == "1050585131");
408 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
410 DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
411 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
412 g_assert(location.altitude().toString() == "545.4");
413 g_assert(location.latitude().toString() == "48.1173");
415 location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
416 DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
417 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
419 //Test incomplete message:
420 location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
421 DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
422 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
424 std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
426 g_assert(checksum(testChecksuming));
428 std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
429 std::string multimessage2 = "57383,W,";
430 std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
431 std::string multimessage4 = "GPGSA,A,";
432 std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
433 std::string multimessage6 = ",7.53,1.00*";
434 std::string multimessage7 = "0E";
436 bool multimessageParse = false;
438 multimessageParse |= tryParse(multimessage1);
439 multimessageParse |= tryParse(multimessage2);
440 multimessageParse |= tryParse(multimessage3);
441 multimessageParse |= tryParse(multimessage4);
442 multimessageParse |= tryParse(multimessage5);
443 multimessageParse |= tryParse(multimessage6);
444 multimessageParse |= tryParse(multimessage7);
446 g_assert(multimessageParse);
448 //test false message:
450 g_assert(!checksum("GPRMC,172758.296,V"));
454 std::string btaddapter = config["bluetoothAdapter"];
456 if(config.find("baudrate")!= config.end())
458 baudrate = boost::lexical_cast<int>( config["baudrate"] );
461 if(config.find("device")!= config.end())
463 std::string dev = config["device"];
464 if(dev.find(":") != string::npos)
467 dev = bt.getDeviceForAddress(dev, btaddapter);
470 device = new SerialPort(dev);
474 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
475 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
480 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
485 DebugOut()<<"read from device: "<<device->read()<<endl;
487 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
488 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
489 g_io_channel_set_close_on_unref(chan, true);
490 g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
494 GpsNmeaSource::~GpsNmeaSource()
499 const string GpsNmeaSource::uuid()
505 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
507 DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
509 std::list<AbstractPropertyType*> f = location->fix();
511 for(auto property : f)
513 if(property->name == reply->property)
515 reply->success = true;
516 reply->value = property;
517 reply->completed(reply);
522 reply->success = false;
523 reply->error = AsyncPropertyReply::InvalidOperation;
524 reply->completed(reply);
527 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
532 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
537 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
539 mRequests.push_back(property);
542 PropertyList GpsNmeaSource::supported()
547 int GpsNmeaSource::supportedOperations()
552 void GpsNmeaSource::canHasData()
554 std::string data = device->read();
559 bool GpsNmeaSource::tryParse(string data)
561 std::vector<std::string> lines;
563 boost::split(lines, data, boost::is_any_of("$"));
565 bool weFoundAMessage = false;
567 for(auto line : lines)
578 std::string::size_type pos = buffer.find('G');
580 if(pos != std::string::npos && pos != 0)
582 ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
583 buffer = buffer.substr(pos);
588 /// we have a complete message. parse it!
589 DebugOut(7)<<"Complete message: "<<buffer<<endl;
590 location->parse(buffer);
591 weFoundAMessage = true;
598 uint cs = buffer.find('*');
599 if (cs != std::string::npos && cs != buffer.length()-1)
601 ///This means we have a false flag somewhere.
602 buffer = buffer.substr(cs+(buffer.length() - cs));
607 DebugOut(7)<<"buffer: "<<buffer<<endl;
610 return weFoundAMessage;
613 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
615 removeOne(&mRequests,property);
618 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
620 mSupported.push_back(property);
622 std::list<Zone::Type> zones;
624 zones.push_back(zone);
626 PropertyInfo info(0, zones);
628 propertyInfoMap[property] = info;
631 bool GpsNmeaSource::checksum(std::string sentence)
633 if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
640 for(auto i : sentence)
644 if(i != '\n' || i != '\r')
648 std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
652 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
654 return sentenceCheck == checksum;