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>
25 #include <superptr.hpp>
26 #include <ambplugin.h>
28 #include <dbusplugin.h>
29 #include <dbusexport.h>
32 #include <boost/assert.hpp>
33 #include <boost/algorithm/string.hpp>
40 #include "abstractpropertytype.h"
42 #define GPSTIME "GpsTime"
43 #define GPSSPEED "GpsSpeed"
44 #define GpsFix "GpsFix"
45 #define GpsSatsUsed "GpsSatsUsed"
46 #define GpsNmea "GpsNmea"
49 inline T2 lexical_cast(const std::string &in) {
57 class GpsInfo: public DBusSink
60 GpsInfo(VehicleProperty::Property, AbstractRoutingEngine* re, GDBusConnection* connection)
61 :DBusSink("GpsInfo", re, connection, map<string, string>())
63 wantPropertyVariant(GPSTIME, "GpsTime", VariantType::Read);
64 wantPropertyVariant(GPSSPEED, "Speed", VariantType::Read);
65 wantPropertyVariant(GpsFix, "Fix", VariantType::Read);
66 wantPropertyVariant(GpsSatsUsed, "SattelitesUsed", VariantType::Read);
67 wantPropertyVariant(GpsNmea, "RawNmea", VariantType::Read);
80 Location(AmbPluginImpl *source, std::shared_ptr<AbstractPropertyType> lat,
81 std::shared_ptr<AbstractPropertyType> lon,
82 std::shared_ptr<AbstractPropertyType> alt,
83 std::shared_ptr<AbstractPropertyType> dir,
84 std::shared_ptr<AbstractPropertyType> spd,
85 std::shared_ptr<AbstractPropertyType> time,
86 std::shared_ptr<AbstractPropertyType> fix,
87 std::shared_ptr<AbstractPropertyType> satsUsed,
88 std::shared_ptr<AbstractPropertyType> vspd);
90 void parse(std::string gprmc);
92 AbstractPropertyType * latitude()
94 return mLatitude.get();
97 AbstractPropertyType * longitude()
99 return mLongitude.get();
102 AbstractPropertyType * altitude()
104 return mAltitude.get();
107 AbstractPropertyType * direction()
109 return mDirection.get();
112 AbstractPropertyType * speed()
117 AbstractPropertyType * gpsTime()
119 return mGpsTime.get();
122 std::list<AbstractPropertyType*> fix()
124 std::list<AbstractPropertyType*> l;
126 l.push_back(mLatitude.get());
127 l.push_back(mLongitude.get());
128 l.push_back(mAltitude.get());
129 l.push_back(mDirection.get());
130 l.push_back(mSpeed.get());
131 l.push_back(mGpsTime.get());
138 void parseGprmc(string gprmc);
139 void parseGpgga(string gpgga);
140 void parseGpgsa(string gpgsa);
142 void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
143 void parseLatitude(std::string d, std::string m, std::string ns);
144 void parseLongitude(std::string d, string m, string ew);
145 void parseSpeed(std::string spd);
146 void parseDirection(std::string dir);
147 void parseAltitude(std::string alt);
149 double degsToDecimal(double degs);
153 std::shared_ptr<AbstractPropertyType> mLatitude;
154 std::shared_ptr<AbstractPropertyType> mLongitude;
155 std::shared_ptr<AbstractPropertyType> mAltitude;
156 std::shared_ptr<AbstractPropertyType> mDirection;
157 std::shared_ptr<AbstractPropertyType> mSpeed;
158 std::shared_ptr<AbstractPropertyType> mVehicleSpeed;
159 std::shared_ptr<AbstractPropertyType> mGpsTime;
160 std::shared_ptr<AbstractPropertyType> mFix;
161 std::shared_ptr<AbstractPropertyType> mSatelitesUsed;
165 AmbPluginImpl * parent;
169 Location::Location(AmbPluginImpl* source,
170 std::shared_ptr<AbstractPropertyType> lat,
171 std::shared_ptr<AbstractPropertyType> lon,
172 std::shared_ptr<AbstractPropertyType> alt,
173 std::shared_ptr<AbstractPropertyType> dir,
174 std::shared_ptr<AbstractPropertyType> spd,
175 std::shared_ptr<AbstractPropertyType> time,
176 std::shared_ptr<AbstractPropertyType> fix,
177 std::shared_ptr<AbstractPropertyType> satsUsed,
178 std::shared_ptr<AbstractPropertyType> vspd)
179 :parent(source), isActive(false)
188 mSatelitesUsed = satsUsed;
189 mVehicleSpeed = vspd;
192 void Location::parse(string nmea)
194 if(boost::algorithm::starts_with(nmea,"GPRMC") || boost::algorithm::starts_with(nmea,"GNRMC"))
198 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
202 else if(boost::algorithm::starts_with(nmea, "GPGSA"))
208 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
212 void Location::parseGprmc(string gprmc)
214 DebugOut(7)<<"parsing gprmc message"<<endl;
216 std::vector<std::string> tokens;
217 boost::split(tokens, gprmc, boost::is_any_of(","));
229 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())
234 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));
236 parseLatitude(tokens[3], "", tokens[4]);
237 parseLongitude(tokens[5], "", tokens[6]);
238 parseSpeed(tokens[7]);
239 parseDirection(tokens[8]);
243 void Location::parseGpgga(string gpgga)
246 std::vector<std::string> tokens;
247 boost::split(tokens, gpgga, boost::is_any_of(","));
249 if(tokens.size() != 15)
251 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
255 parseLatitude(tokens[2],"",tokens[3]);
256 parseLongitude(tokens[4],"",tokens[5]);
261 else isActive = false;
263 parseAltitude(tokens[9]);
266 void Location::parseGpgsa(string gpgsa)
268 std::vector<std::string> tokens;
269 boost::split(tokens, gpgsa, boost::is_any_of(","));
271 if(tokens.size() != 18)
273 DebugOut()<<"Invalid GPGSA message: "<<gpgsa<<endl;
277 Location::FixType fix = (Location::FixType)boost::lexical_cast<int>(tokens[2]);
279 uint16_t numsats = 0;
281 for(int i=3; i<15; i++)
283 std::string sat = tokens[i];
290 if(mFix->value<Location::FixType>() != fix)
291 parent->setValue(mFix, fix);
293 if(mSatelitesUsed->value<uint16_t>() != numsats)
294 parent->setValue(mSatelitesUsed, numsats);
297 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
302 t.tm_hour = boost::lexical_cast<int>(h);
303 t.tm_min = boost::lexical_cast<int>(m);
304 t.tm_sec = boost::lexical_cast<int>(s);
305 t.tm_mday = boost::lexical_cast<int>(dd);
306 t.tm_mon = boost::lexical_cast<int>(mm) - 1;
307 t.tm_year = boost::lexical_cast<int>(yy) + 100;
309 time_t time = mktime(&t);
311 if(mGpsTime->value<double>() != (double(time)))
313 parent->setValue(mGpsTime, double(time));
318 DebugOut(5)<<"Failed to parse time "<<endl;
322 void Location::parseLatitude(string d, string m, string ns)
329 double degs = boost::lexical_cast<double>(d + m);
330 double dec = degsToDecimal(degs);
335 if(mLatitude->value<double>() != dec)
337 parent->setValue(mLatitude, dec);
342 DebugOut(5)<<"Failed to parse latitude"<<endl;
346 void Location::parseLongitude(string d, string m, string ew)
350 if(d.empty()) return;
352 double degs = boost::lexical_cast<double>(d + m);
353 double dec = degsToDecimal(degs);
358 if(mLongitude->value<double>() != dec)
360 parent->setValue(mLongitude, dec);
365 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
369 void Location::parseSpeed(string spd)
373 double s = boost::lexical_cast<double>(spd);
378 uint16_t speed = static_cast<uint16_t>(s);
380 if(mSpeed->value<uint16_t>() != speed)
382 parent->setValue(mSpeed, speed);
387 DebugOut(5)<<"failed to parse speed"<<endl;
391 void Location::parseDirection(string dir)
394 uint16_t d = boost::lexical_cast<double>(dir);
396 if(mDirection->value<uint16_t>() != d)
398 parent->setValue(mDirection, d);
403 DebugOut(5) << "Failed to parse direction: " << dir << endl;
407 void Location::parseAltitude(string alt)
411 if(alt.empty()) return;
413 double a = boost::lexical_cast<double>(alt);
415 if(mAltitude->value<double>() != a)
417 parent->setValue(mAltitude, a);
422 DebugOut(5)<<"failed to parse altitude"<<endl;
426 double Location::degsToDecimal(double degs)
429 double min = 100.0 * modf(degs / 100.0, °);
430 return deg + (min / 60.0);
433 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
435 // DebugOut(5) << "Polling..." << condition << endl;
437 if(condition & G_IO_ERR)
439 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
442 if (condition & G_IO_HUP)
444 //Hang up. Returning false closes out the GIOChannel.
445 //printf("Callback on G_IO_HUP\n");
446 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
450 GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
457 extern "C" void create(AbstractRoutingEngine* routingengine, map<string, string> config)
459 auto plugin = new AmbPlugin<GpsNmeaSource>(routingengine, config);
463 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config, AbstractSource &parent)
464 :AmbPluginImpl(re, config, parent), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr), bt(nullptr)
468 auto lat = addPropertySupport<VehicleProperty::LatitudeType>(Zone::None);
469 auto lon = addPropertySupport<VehicleProperty::LongitudeType>(Zone::None);
470 auto alt = addPropertySupport<VehicleProperty::AltitudeType>(Zone::None);
471 auto spd = addPropertySupport(Zone::None, [](){ return new BasicPropertyType<uint16_t>(GPSSPEED, 0); });
472 auto vspd = addPropertySupport<VehicleProperty::VehicleSpeedType>(Zone::None);
473 auto dir = addPropertySupport<VehicleProperty::DirectionType>(Zone::None);
474 auto time = addPropertySupport(Zone::None, [](){ return new BasicPropertyType<double>(GPSTIME, 0); });
475 auto fix = addPropertySupport(Zone::None, []() { return new BasicPropertyType<Location::FixType>(GpsFix, Location::NoFix); });
476 auto satsUsed = addPropertySupport(Zone::None, []() { return new BasicPropertyType<uint16_t>(GpsSatsUsed, 0); });
477 rawNmea = addPropertySupport(Zone::None, []() { return new StringPropertyType(GpsNmea); });
479 location = new Location(this, lat, lon, alt, dir, spd, time, fix, satsUsed, vspd);
481 std::string btaddapter = config["bluetoothAdapter"];
483 if(config.find("baudrate")!= config.end())
485 baudrate = boost::lexical_cast<int>( config["baudrate"] );
488 if(config.find("device")!= config.end())
490 std::string dev = config["device"];
492 if(dev.find(":") != string::npos)
495 bt = new Bluetooth5();
496 bt->getDeviceForAddress(dev, [this](int fd) {
497 DebugOut() << "fd: " << fd << endl;
498 device = new SerialPort(fd);
503 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
504 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
507 DebugOut()<<"read from device: "<<device->read()<<endl;
509 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
510 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
511 g_io_channel_set_close_on_unref(chan, true);
512 g_io_channel_unref(chan);
515 bt = new BluetoothDevice();
516 dev = bt->getDeviceForAddress(dev, btaddapter);
518 device = new SerialPort(dev);
522 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
523 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
528 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
533 DebugOut()<<"read from device: "<<device->read()<<endl;
535 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
536 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
537 g_io_channel_set_close_on_unref(chan, true);
538 g_io_channel_unref(chan);
543 device = new SerialPort(dev);
547 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
548 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
553 DebugOut(DebugOut::Error) << "Failed to open gps tty: " << config["device"] << endl;
558 DebugOut()<<"read from device: "<<device->read()<<endl;
560 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
561 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR), (GIOFunc)readCallback, this);
562 g_io_channel_set_close_on_unref(chan, true);
563 g_io_channel_unref(chan);
568 GpsNmeaSource::~GpsNmeaSource()
570 if(device && device->isOpen())
576 const string GpsNmeaSource::uuid() const
581 int GpsNmeaSource::supportedOperations() const
583 return AbstractSource::Get;
586 void GpsNmeaSource::init()
588 if(configuration.find("test") != configuration.end())
593 routingEngine->subscribeToProperty(DBusConnected,[this](AbstractPropertyType* value)
595 if(value->name == DBusConnected)
597 if(value->value<bool>())
599 amb::Exporter::instance()->exportProperty<GpsInfo>(routingEngine);
605 void GpsNmeaSource::canHasData()
607 std::string data = device->read();
612 void GpsNmeaSource::test()
614 location->parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
616 DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
617 DebugOut(0)<<"lat: "<<location->gpsTime()->toString()<<endl;
619 g_assert(location->latitude()->toString() == "-23.86600833");
620 g_assert(location->gpsTime()->toString() == "1050585131");
622 location->parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
624 DebugOut(0)<<"alt: "<<location->altitude()->toString()<<endl;
625 DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
626 g_assert(location->altitude()->toString() == "545.4");
627 g_assert(location->latitude()->toString() == "48.1173");
629 location->parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
630 DebugOut(0)<<"lon: "<<location->longitude()->toString()<<endl;
631 DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
634 location->parse("GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39");
636 //Test incomplete message:
637 location->parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
638 DebugOut(0)<<"lon: "<<location->longitude()->toString()<<endl;
639 DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
641 std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
643 g_assert(checksum(testChecksuming));
645 std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
646 std::string multimessage2 = "57383,W,";
647 std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
648 std::string multimessage4 = "GPGSA,A,";
649 std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
650 std::string multimessage6 = ",7.53,1.00*";
651 std::string multimessage7 = "0E";
653 bool multimessageParse = false;
655 multimessageParse |= tryParse(multimessage1);
656 multimessageParse |= tryParse(multimessage2);
657 multimessageParse |= tryParse(multimessage3);
658 multimessageParse |= tryParse(multimessage4);
659 multimessageParse |= tryParse(multimessage5);
660 multimessageParse |= tryParse(multimessage6);
661 multimessageParse |= tryParse(multimessage7);
663 g_assert(multimessageParse);
665 //Test meaningingless message:
666 location->parse("GPRMC,,V,,,,,,,,,,N*53");
668 //test false message:
670 g_assert(!checksum("GPRMC,172758.296,V"));
673 bool GpsNmeaSource::tryParse(string data)
675 std::vector<std::string> lines;
677 boost::split(lines, data, boost::is_any_of("$"));
679 bool weFoundAMessage = false;
681 for(auto line : lines)
692 std::string::size_type pos = buffer.find('G');
694 if(pos != std::string::npos && pos != 0)
696 ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
697 buffer = buffer.substr(pos);
702 /// we have a complete message. parse it!
703 DebugOut(7)<<"Complete message: "<<buffer<<endl;
704 location->parse(buffer);
705 boost::algorithm::erase_all(buffer, "\n");
706 boost::algorithm::erase_all(buffer, "\r");
707 setValue(rawNmea, buffer);
708 weFoundAMessage = true;
715 std::string::size_type cs = buffer.find('*');
716 if (cs != std::string::npos && cs != buffer.length()-1)
718 buffer = buffer.substr(cs+(buffer.length() - cs));
723 DebugOut(7)<<"buffer: "<<buffer<<endl;
726 return weFoundAMessage;
729 bool GpsNmeaSource::checksum(std::string sentence)
731 if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
738 for(auto i : sentence)
742 if(i != '\n' || i != '\r')
746 std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
750 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
752 return sentenceCheck == checksum;