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 "bluetooth5.h"
24 #include <listplusplus.h>
27 #include <boost/assert.hpp>
28 #include <boost/algorithm/string.hpp>
35 #include "abstractpropertytype.h"
37 #define GPSTIME "GpsTime"
40 inline T2 lexical_cast(const std::string &in) {
51 Location(AbstractRoutingEngine* re, std::string uuid);
53 void parse(std::string gprmc);
55 VehicleProperty::LatitudeType latitude()
60 VehicleProperty::LongitudeType longitude()
65 VehicleProperty::AltitudeType altitude()
70 VehicleProperty::DirectionType direction()
75 VehicleProperty::VehicleSpeedType speed()
80 BasicPropertyType<double> gpsTime()
85 std::list<AbstractPropertyType*> fix()
87 std::list<AbstractPropertyType*> l;
89 l.push_back(&mLatitude);
90 l.push_back(&mLongitude);
91 l.push_back(&mAltitude);
92 l.push_back(&mDirection);
94 l.push_back(&mGpsTime);
101 void parseGprmc(string gprmc);
102 void parseGpgga(string gpgga);
104 void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
105 void parseLatitude(std::string d, std::string m, std::string ns);
106 void parseLongitude(std::string d, string m, string ew);
107 void parseSpeed(std::string spd);
108 void parseDirection(std::string dir);
109 void parseAltitude(std::string alt);
111 double degsToDecimal(double degs);
115 VehicleProperty::LatitudeType mLatitude;
116 VehicleProperty::LongitudeType mLongitude;
117 VehicleProperty::AltitudeType mAltitude;
118 VehicleProperty::DirectionType mDirection;
119 VehicleProperty::VehicleSpeedType mSpeed;
120 BasicPropertyType<double> mGpsTime;
126 AbstractRoutingEngine* routingEngine;
130 Location::Location(AbstractRoutingEngine* re, std::string uuid)
131 :mLatitude(0), mLongitude(0), mAltitude(0), mDirection(0), mSpeed(0), mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
136 void Location::parse(string nmea)
138 if(boost::algorithm::starts_with(nmea,"GPRMC"))
142 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
148 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
152 void Location::parseGprmc(string gprmc)
154 DebugOut(7)<<"parsing gprmc message"<<endl;
156 std::vector<std::string> tokens;
157 boost::split(tokens, gprmc, boost::is_any_of(","));
169 if(tokens[1].empty() || tokens[9].empty() || tokens[3].empty() || tokens[4].empty() || tokens[5].empty() || tokens[6].empty() || tokens[7].empty() || tokens[8].empty())
174 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));
176 parseLatitude(tokens[3], "", tokens[4]);
177 parseLongitude(tokens[5], "", tokens[6]);
178 parseSpeed(tokens[7]);
179 parseDirection(tokens[8]);
183 void Location::parseGpgga(string gpgga)
186 std::vector<std::string> tokens;
187 boost::split(tokens, gpgga, boost::is_any_of(","));
189 if(tokens.size() != 15)
191 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
195 parseLatitude(tokens[2],"",tokens[3]);
196 parseLongitude(tokens[4],"",tokens[5]);
201 else isActive = false;
203 parseAltitude(tokens[9]);
206 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
211 t.tm_hour = boost::lexical_cast<int>(h);
212 t.tm_min = boost::lexical_cast<int>(m);
213 t.tm_sec = boost::lexical_cast<int>(s);
214 t.tm_mday = boost::lexical_cast<int>(dd);
215 t.tm_mon = boost::lexical_cast<int>(mm);
216 t.tm_year = boost::lexical_cast<int>(yy) + 100;
218 time_t time = mktime(&t);
220 BasicPropertyType<double> temp(GPSTIME,(double)time);
226 routingEngine->updateProperty(&mGpsTime, mUuid);
231 DebugOut(5)<<"Failed to parse time "<<endl;
235 void Location::parseLatitude(string d, string m, string ns)
242 double degs = boost::lexical_cast<double>(d + m);
243 double dec = degsToDecimal(degs);
248 VehicleProperty::LatitudeType temp(dec);
250 if(mLatitude != temp)
255 routingEngine->updateProperty(&mLatitude, mUuid);
260 DebugOut(5)<<"Failed to parse latitude"<<endl;
264 void Location::parseLongitude(string d, string m, string ew)
268 if(d.empty()) return;
270 double degs = boost::lexical_cast<double>(d + m);
271 double dec = degsToDecimal(degs);
276 VehicleProperty::LongitudeType temp(dec);
278 if(mLongitude != temp)
283 routingEngine->updateProperty(&mLongitude, mUuid);
288 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
292 void Location::parseSpeed(string spd)
296 double s = boost::lexical_cast<double>(spd);
300 VehicleProperty::VehicleSpeedType temp(s);
306 routingEngine->updateProperty(&mSpeed, mUuid);
311 DebugOut(5)<<"failed to parse speed"<<endl;
315 void Location::parseDirection(string dir)
318 uint16_t d = boost::lexical_cast<double>(dir);
320 VehicleProperty::DirectionType temp(d);
321 if(mDirection != temp)
325 routingEngine->updateProperty(&mDirection, mUuid);
330 DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
334 void Location::parseAltitude(string alt)
338 if(alt.empty()) return;
340 double a = boost::lexical_cast<double>(alt);
342 VehicleProperty::AltitudeType temp(a);
343 if(mAltitude != temp)
348 routingEngine->updateProperty(&mAltitude, mUuid);
351 mAltitude = VehicleProperty::AltitudeType(a);
355 DebugOut(5)<<"failed to parse altitude"<<endl;
359 double Location::degsToDecimal(double degs)
362 double min = 100.0 * modf(degs / 100.0, °);
363 return deg + (min / 60.0);
366 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
368 // DebugOut(5) << "Polling..." << condition << endl;
370 if(condition & G_IO_ERR)
372 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
375 if (condition & G_IO_HUP)
377 //Hang up. Returning false closes out the GIOChannel.
378 //printf("Callback on G_IO_HUP\n");
379 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
383 GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
390 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
392 return new GpsNmeaSource(routingengine, config);
396 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
397 :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr), bt(nullptr)
400 location =new Location(re, mUuid);
402 VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
404 addPropertySupport(VehicleProperty::Latitude, Zone::None);
405 addPropertySupport(VehicleProperty::Longitude, Zone::None);
406 addPropertySupport(VehicleProperty::Altitude, Zone::None);
407 addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
408 addPropertySupport(VehicleProperty::Direction, Zone::None);
409 addPropertySupport(GPSTIME, Zone::None);
413 if(config.find("test") != config.end())
418 std::string btaddapter = config["bluetoothAdapter"];
420 if(config.find("baudrate")!= config.end())
422 baudrate = boost::lexical_cast<int>( config["baudrate"] );
425 if(config.find("device")!= config.end())
427 std::string dev = config["device"];
430 if(dev.find(":") != string::npos)
433 bt = new Bluetooth5();
434 bt->getDeviceForAddress(dev, [this](int fd) {
435 DebugOut() << "fd: " << fd << endl;
436 device = new SerialPort(fd);
441 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
442 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
445 DebugOut()<<"read from device: "<<device->read()<<endl;
447 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
448 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
449 g_io_channel_set_close_on_unref(chan, true);
450 g_io_channel_unref(chan);
453 bt = new BluetoothDevice();
454 dev = bt->getDeviceForAddress(dev, btaddapter);
456 device = new SerialPort(dev);
460 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
461 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
466 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
471 DebugOut()<<"read from device: "<<device->read()<<endl;
473 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
474 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
475 g_io_channel_set_close_on_unref(chan, true);
476 g_io_channel_unref(chan);
481 device = new SerialPort(dev);
485 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
486 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
491 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
496 DebugOut()<<"read from device: "<<device->read()<<endl;
498 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
499 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR), (GIOFunc)readCallback, this);
500 g_io_channel_set_close_on_unref(chan, true);
501 g_io_channel_unref(chan);
506 GpsNmeaSource::~GpsNmeaSource()
508 if(device && device->isOpen())
514 const string GpsNmeaSource::uuid()
520 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
522 DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
524 std::list<AbstractPropertyType*> f = location->fix();
526 for(auto property : f)
528 if(property->name == reply->property)
530 reply->success = true;
531 reply->value = property;
532 reply->completed(reply);
537 reply->success = false;
538 reply->error = AsyncPropertyReply::InvalidOperation;
539 reply->completed(reply);
542 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
547 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
552 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
554 mRequests.push_back(property);
557 PropertyList GpsNmeaSource::supported()
562 int GpsNmeaSource::supportedOperations()
567 void GpsNmeaSource::canHasData()
569 std::string data = device->read();
574 void GpsNmeaSource::test()
576 Location location(nullptr, "");
577 location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
579 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
581 g_assert(location.latitude().toString() == "-23.86600833");
582 g_assert(location.gpsTime().toString() == "1050585131");
584 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
586 DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
587 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
588 g_assert(location.altitude().toString() == "545.4");
589 g_assert(location.latitude().toString() == "48.1173");
591 location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
592 DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
593 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
595 //Test incomplete message:
596 location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
597 DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
598 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
600 std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
602 g_assert(checksum(testChecksuming));
604 std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
605 std::string multimessage2 = "57383,W,";
606 std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
607 std::string multimessage4 = "GPGSA,A,";
608 std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
609 std::string multimessage6 = ",7.53,1.00*";
610 std::string multimessage7 = "0E";
612 bool multimessageParse = false;
614 multimessageParse |= tryParse(multimessage1);
615 multimessageParse |= tryParse(multimessage2);
616 multimessageParse |= tryParse(multimessage3);
617 multimessageParse |= tryParse(multimessage4);
618 multimessageParse |= tryParse(multimessage5);
619 multimessageParse |= tryParse(multimessage6);
620 multimessageParse |= tryParse(multimessage7);
622 g_assert(multimessageParse);
624 //Test meaningingless message:
625 location.parse("GPRMC,,V,,,,,,,,,,N*53");
627 //test false message:
629 g_assert(!checksum("GPRMC,172758.296,V"));
632 bool GpsNmeaSource::tryParse(string data)
634 std::vector<std::string> lines;
636 boost::split(lines, data, boost::is_any_of("$"));
638 bool weFoundAMessage = false;
640 for(auto line : lines)
651 std::string::size_type pos = buffer.find('G');
653 if(pos != std::string::npos && pos != 0)
655 ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
656 buffer = buffer.substr(pos);
661 /// we have a complete message. parse it!
662 DebugOut(7)<<"Complete message: "<<buffer<<endl;
663 location->parse(buffer);
664 weFoundAMessage = true;
671 uint cs = buffer.find('*');
672 if (cs != std::string::npos && cs != buffer.length()-1)
674 ///This means we have a false flag somewhere.
675 buffer = buffer.substr(cs+(buffer.length() - cs));
680 DebugOut(7)<<"buffer: "<<buffer<<endl;
683 return weFoundAMessage;
686 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
688 removeOne(&mRequests,property);
691 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
693 mSupported.push_back(property);
695 std::list<Zone::Type> zones;
697 zones.push_back(zone);
699 PropertyInfo info(0, zones);
701 propertyInfoMap[property] = info;
704 bool GpsNmeaSource::checksum(std::string sentence)
706 if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
713 for(auto i : sentence)
717 if(i != '\n' || i != '\r')
721 std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
725 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
727 return sentenceCheck == checksum;
739 int main(int argc, char** argv)
741 DebugOut::setDebugThreshhold(7);
742 GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());