plugins/gpsnmea/gpsnmea.cpp

   1 /*
   2 Copyright (C) 2012 Intel Corporation
   3
   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.
   8
   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.
  13
  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
  17 */
  18
  19 #include "gpsnmea.h"
  20 #include "timestamp.h"
  21 #include "serialport.hpp"
  22 #include "bluetooth.hpp"
  23 #include "bluetooth5.h"
  24 #include <listplusplus.h>
  25
  26 #include <iostream>
  27 #include <boost/assert.hpp>
  28 #include <boost/algorithm/string.hpp>
  29 #include <time.h>
  30
  31
  32 using namespace std;
  33
  34 #include "debugout.h"
  35 #include "abstractpropertytype.h"
  36
  37 #define GPSTIME "GpsTime"
  38
  39 template<typename T2>
  40 inline T2 lexical_cast(const std::string &in) {
  41         T2 out;
  42         std::stringstream ss;
  43         ss << std::hex << in;
  44         ss >> out;
  45         return out;
  46 }
  47
  48 class Location
  49 {
  50 public:
  51         Location(AbstractRoutingEngine* re, std::string uuid);
  52
  53         void parse(std::string gprmc);
  54
  55         VehicleProperty::LatitudeType latitude()
  56         {
  57                 return mLatitude;
  58         }
  59
  60         VehicleProperty::LongitudeType longitude()
  61         {
  62                 return mLongitude;
  63         }
  64
  65         VehicleProperty::AltitudeType altitude()
  66         {
  67                 return mAltitude;
  68         }
  69
  70         VehicleProperty::DirectionType direction()
  71         {
  72                 return mDirection;
  73         }
  74
  75         VehicleProperty::VehicleSpeedType speed()
  76         {
  77                 return mSpeed;
  78         }
  79
  80         BasicPropertyType<double> gpsTime()
  81         {
  82                 return mGpsTime;
  83         }
  84
  85         std::list<AbstractPropertyType*> fix()
  86         {
  87                 std::list<AbstractPropertyType*> l;
  88
  89                 l.push_back(&mLatitude);
  90                 l.push_back(&mLongitude);
  91                 l.push_back(&mAltitude);
  92                 l.push_back(&mDirection);
  93                 l.push_back(&mSpeed);
  94                 l.push_back(&mGpsTime);
  95
  96                 return l;
  97         }
  98
  99 private: ///methods:
 100
 101         void parseGprmc(string gprmc);
 102         void parseGpgga(string gpgga);
 103
 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);
 110
 111         double degsToDecimal(double degs);
 112
 113 private:
 114
 115         VehicleProperty::LatitudeType mLatitude;
 116         VehicleProperty::LongitudeType mLongitude;
 117         VehicleProperty::AltitudeType mAltitude;
 118         VehicleProperty::DirectionType  mDirection;
 119         VehicleProperty::VehicleSpeedType mSpeed;
 120         BasicPropertyType<double> mGpsTime;
 121
 122         bool isActive;
 123
 124         std::string mUuid;
 125
 126         AbstractRoutingEngine* routingEngine;
 127
 128 };
 129
 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)
 132 {
 133
 134 }
 135
 136 void Location::parse(string nmea)
 137 {
 138         if(boost::algorithm::starts_with(nmea,"GPRMC"))
 139         {
 140                 parseGprmc(nmea);
 141         }
 142         else if(boost::algorithm::starts_with(nmea,"GPGGA"))
 143         {
 144                 parseGpgga(nmea);
 145         }
 146         else
 147         {
 148                 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
 149         }
 150 }
 151
 152 void Location::parseGprmc(string gprmc)
 153 {
 154         DebugOut(7)<<"parsing gprmc message"<<endl;
 155
 156         std::vector<std::string> tokens;
 157         boost::split(tokens, gprmc, boost::is_any_of(","));
 158
 159         if(!tokens.size())
 160         {
 161                 return;
 162         }
 163
 164         if(tokens[2] == "A")
 165         {
 166                 isActive = true;
 167         }
 168
 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())
 170         {
 171                 return;
 172         }
 173
 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));
 175
 176         parseLatitude(tokens[3], "", tokens[4]);
 177         parseLongitude(tokens[5], "", tokens[6]);
 178         parseSpeed(tokens[7]);
 179         parseDirection(tokens[8]);
 180
 181 }
 182
 183 void Location::parseGpgga(string gpgga)
 184 {
 185
 186         std::vector<std::string> tokens;
 187         boost::split(tokens, gpgga, boost::is_any_of(","));
 188
 189         if(tokens.size() != 15)
 190         {
 191                 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
 192                 return;
 193         }
 194
 195         parseLatitude(tokens[2],"",tokens[3]);
 196         parseLongitude(tokens[4],"",tokens[5]);
 197         if(tokens[6] != "0")
 198         {
 199                 isActive = true;
 200         }
 201         else isActive = false;
 202
 203         parseAltitude(tokens[9]);
 204 }
 205
 206 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
 207 {
 208         try
 209         {
 210                 tm t;
 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;
 217
 218                 time_t time = mktime(&t);
 219
 220                 BasicPropertyType<double> temp(GPSTIME,(double)time);
 221
 222                 if(mGpsTime != temp)
 223                 {
 224                         mGpsTime = temp;
 225                         if(routingEngine)
 226                                 routingEngine->updateProperty(&mGpsTime, mUuid);
 227                 }
 228         }
 229         catch(...)
 230         {
 231                 DebugOut(5)<<"Failed to parse time "<<endl;
 232         }
 233 }
 234
 235 void Location::parseLatitude(string d, string m, string ns)
 236 {
 237         try
 238         {
 239                 if(d.empty() )
 240                         return;
 241
 242                 double degs = boost::lexical_cast<double>(d + m);
 243                 double dec = degsToDecimal(degs);
 244
 245                 if(ns == "S")
 246                         dec *= -1;
 247
 248                 VehicleProperty::LatitudeType temp(dec);
 249
 250                 if(mLatitude != temp)
 251                 {
 252                         mLatitude = temp;
 253
 254                         if(routingEngine)
 255                                 routingEngine->updateProperty(&mLatitude, mUuid);
 256                 }
 257         }
 258         catch(...)
 259         {
 260                 DebugOut(5)<<"Failed to parse latitude"<<endl;
 261         }
 262 }
 263
 264 void Location::parseLongitude(string d, string m, string ew)
 265 {
 266         try
 267         {
 268                 if(d.empty()) return;
 269
 270                 double degs = boost::lexical_cast<double>(d + m);
 271                 double dec = degsToDecimal(degs);
 272
 273                 if(ew == "W")
 274                         dec *= -1;
 275
 276                 VehicleProperty::LongitudeType temp(dec);
 277
 278                 if(mLongitude != temp)
 279                 {
 280                         mLongitude = temp;
 281
 282                         if(routingEngine)
 283                                 routingEngine->updateProperty(&mLongitude, mUuid);
 284                 }
 285         }
 286         catch(...)
 287         {
 288                 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
 289         }
 290 }
 291
 292 void Location::parseSpeed(string spd)
 293 {
 294         try
 295         {
 296                 double s = boost::lexical_cast<double>(spd);
 297
 298                 ///to kph:
 299                 s *= 1.852;
 300                 VehicleProperty::VehicleSpeedType temp(s);
 301                 if(mSpeed != temp)
 302                 {
 303                         mSpeed = temp;
 304
 305                         if(routingEngine)
 306                                 routingEngine->updateProperty(&mSpeed, mUuid);
 307                 }
 308         }
 309         catch(...)
 310         {
 311                 DebugOut(5)<<"failed to parse speed"<<endl;
 312         }
 313 }
 314
 315 void Location::parseDirection(string dir)
 316 {
 317         try {
 318                 uint16_t d = boost::lexical_cast<double>(dir);
 319
 320                 VehicleProperty::DirectionType temp(d);
 321                 if(mDirection != temp)
 322                 {
 323                         mDirection = temp;
 324                         if(routingEngine)
 325                                 routingEngine->updateProperty(&mDirection, mUuid);
 326                 }
 327         }
 328         catch(...)
 329         {
 330                 DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
 331         }
 332 }
 333
 334 void Location::parseAltitude(string alt)
 335 {
 336         try{
 337
 338                 if(alt.empty()) return;
 339
 340                 double a = boost::lexical_cast<double>(alt);
 341
 342                 VehicleProperty::AltitudeType temp(a);
 343                 if(mAltitude != temp)
 344                 {
 345                         mAltitude = temp;
 346
 347                         if(routingEngine)
 348                                 routingEngine->updateProperty(&mAltitude, mUuid);
 349                 }
 350
 351                 mAltitude = VehicleProperty::AltitudeType(a);
 352         }
 353         catch(...)
 354         {
 355                 DebugOut(5)<<"failed to parse altitude"<<endl;
 356         }
 357 }
 358
 359 double Location::degsToDecimal(double degs)
 360 {
 361         double deg;
 362         double min = 100.0 * modf(degs / 100.0, &deg);
 363         return deg + (min / 60.0);
 364 }
 365
 366 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
 367 {
 368 //      DebugOut(5) << "Polling..." << condition << endl;
 369
 370         if(condition & G_IO_ERR)
 371         {
 372                 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
 373         }
 374
 375         if (condition & G_IO_HUP)
 376         {
 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;
 380                 return false;
 381         }
 382
 383         GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
 384
 385         src->canHasData();
 386
 387         return true;
 388 }
 389
 390 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
 391 {
 392         return new GpsNmeaSource(routingengine, config);
 393
 394 }
 395
 396 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
 397         :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr)
 398 {
 399         int baudrate = 0;
 400         location =new Location(re, mUuid);
 401
 402         VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
 403
 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);
 410
 411         ///test:
 412
 413         if(config.find("test") != config.end())
 414         {
 415                 test();
 416         }
 417
 418         std::string btaddapter = config["bluetoothAdapter"];
 419
 420         if(config.find("baudrate")!= config.end())
 421         {
 422                 baudrate = boost::lexical_cast<int>( config["baudrate"] );
 423         }
 424
 425         if(config.find("device")!= config.end())
 426         {
 427                 std::string dev = config["device"];
 428                 if(dev.find(":") != string::npos)
 429                 {
 430 #ifdef USE_BLUEZ5
 431                         Bluetooth5 bt;
 432                         bt.getDeviceForAddress(dev,[this](int fd) {
 433                                 device = new SerialPort(fd);
 434                                 int baudrate=0;
 435
 436                                 if(baudrate!=0)
 437                                 {
 438                                         if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
 439                                                 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
 440                                 }
 441
 442                                 if(!device->open())
 443                                 {
 444                                         DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<configuration["device"]<<endl;
 445                                         perror("Error");
 446                                         return;
 447                                 }
 448
 449                                 DebugOut()<<"read from device: "<<device->read()<<endl;
 450
 451                                 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
 452                                 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
 453                                 g_io_channel_set_close_on_unref(chan, true);
 454                                 g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
 455                         });
 456
 457                 }
 458         }
 459
 460 #else
 461                         BluetoothDevice bt;
 462                         dev = bt.getDeviceForAddress(dev, btaddapter);
 463
 464                 }
 465
 466                 device = new SerialPort(dev);
 467
 468                 if(baudrate!=0)
 469                 {
 470                         if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
 471                                 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
 472                 }
 473
 474                 if(!device->open())
 475                 {
 476                         DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
 477                         perror("Error");
 478                         return;
 479                 }
 480
 481                 DebugOut()<<"read from device: "<<device->read()<<endl;
 482
 483                 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
 484                 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
 485                 g_io_channel_set_close_on_unref(chan, true);
 486                 g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
 487         }
 488 #endif
 489 }
 490
 491 GpsNmeaSource::~GpsNmeaSource()
 492 {
 493         if(device && device->isOpen())
 494                 device->close();
 495 }
 496
 497 const string GpsNmeaSource::uuid()
 498 {
 499         return mUuid;
 500 }
 501
 502
 503 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
 504 {
 505         DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
 506
 507         std::list<AbstractPropertyType*> f = location->fix();
 508
 509         for(auto property : f)
 510         {
 511                 if(property->name == reply->property)
 512                 {
 513                         reply->success = true;
 514                         reply->value = property;
 515                         reply->completed(reply);
 516                         return;
 517                 }
 518         }
 519
 520         reply->success = false;
 521         reply->error = AsyncPropertyReply::InvalidOperation;
 522         reply->completed(reply);
 523 }
 524
 525 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
 526 {
 527
 528 }
 529
 530 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
 531 {
 532
 533 }
 534
 535 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
 536 {
 537         mRequests.push_back(property);
 538 }
 539
 540 PropertyList GpsNmeaSource::supported()
 541 {
 542         return mSupported;
 543 }
 544
 545 int GpsNmeaSource::supportedOperations()
 546 {
 547         return Get;
 548 }
 549
 550 void GpsNmeaSource::canHasData()
 551 {
 552         std::string data = device->read();
 553
 554         tryParse(data);
 555 }
 556
 557 void GpsNmeaSource::test()
 558 {
 559         Location location(nullptr, "");
 560         location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
 561
 562         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
 563
 564         g_assert(location.latitude().toString() == "-23.86600833");
 565         g_assert(location.gpsTime().toString() == "1050585131");
 566
 567         location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
 568
 569         DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
 570         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
 571         g_assert(location.altitude().toString() == "545.4");
 572         g_assert(location.latitude().toString() == "48.1173");
 573
 574         location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
 575         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
 576         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
 577
 578         //Test incomplete message:
 579         location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
 580         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
 581         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
 582
 583         std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
 584
 585         g_assert(checksum(testChecksuming));
 586
 587         std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
 588         std::string multimessage2 = "57383,W,";
 589         std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
 590         std::string multimessage4 = "GPGSA,A,";
 591         std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
 592         std::string multimessage6 = ",7.53,1.00*";
 593         std::string multimessage7 = "0E";
 594
 595         bool multimessageParse = false;
 596
 597         multimessageParse |= tryParse(multimessage1);
 598         multimessageParse |= tryParse(multimessage2);
 599         multimessageParse |= tryParse(multimessage3);
 600         multimessageParse |= tryParse(multimessage4);
 601         multimessageParse |= tryParse(multimessage5);
 602         multimessageParse |= tryParse(multimessage6);
 603         multimessageParse |= tryParse(multimessage7);
 604
 605         g_assert(multimessageParse);
 606
 607         //Test meaningingless message:
 608         location.parse("GPRMC,,V,,,,,,,,,,N*53");
 609
 610         //test false message:
 611
 612         g_assert(!checksum("GPRMC,172758.296,V"));
 613 }
 614
 615 bool GpsNmeaSource::tryParse(string data)
 616 {
 617         std::vector<std::string> lines;
 618
 619         boost::split(lines, data, boost::is_any_of("$"));
 620
 621         bool weFoundAMessage = false;
 622
 623         for(auto line : lines)
 624         {
 625                 if(checksum(line))
 626                 {
 627                         buffer = line;
 628                 }
 629                 else
 630                 {
 631                         buffer += line;
 632                 }
 633
 634                 std::string::size_type pos = buffer.find('G');
 635
 636                 if(pos != std::string::npos && pos != 0)
 637                 {
 638                         ///Throw the incomplete stuff away.  if it doesn't begin with "G" it'll never be complete
 639                         buffer = buffer.substr(pos);
 640                 }
 641
 642                 if(checksum(buffer))
 643                 {
 644                         /// we have a complete message.  parse it!
 645                         DebugOut(7)<<"Complete message: "<<buffer<<endl;
 646                         location->parse(buffer);
 647                         weFoundAMessage = true;
 648                         buffer = "";
 649                 }
 650                 else
 651                 {
 652                         if(pos == 0 )
 653                         {
 654                                 uint cs = buffer.find('*');
 655                                 if (cs != std::string::npos && cs != buffer.length()-1)
 656                                 {
 657                                         ///This means we have a false flag somewhere.
 658                                         buffer = buffer.substr(cs+(buffer.length() - cs));
 659                                 }
 660                         }
 661                 }
 662
 663                 DebugOut(7)<<"buffer: "<<buffer<<endl;
 664         }
 665
 666         return weFoundAMessage;
 667 }
 668
 669 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
 670 {
 671         removeOne(&mRequests,property);
 672 }
 673
 674 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
 675 {
 676         mSupported.push_back(property);
 677
 678         std::list<Zone::Type> zones;
 679
 680         zones.push_back(zone);
 681
 682         PropertyInfo info(0, zones);
 683
 684         propertyInfoMap[property] = info;
 685 }
 686
 687 bool GpsNmeaSource::checksum(std::string sentence)
 688 {
 689         if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
 690         {
 691                 return false;
 692         }
 693
 694         int checksum = 0;
 695
 696         for(auto i : sentence)
 697         {
 698                 if(i == '*')
 699                         break;
 700                 if(i != '\n' || i != '\r')
 701                         checksum ^= i;
 702         }
 703
 704         std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
 705
 706         try
 707         {
 708                 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
 709
 710                 return sentenceCheck == checksum;
 711         }
 712         catch(...)
 713
 714         {
 715                 return false;
 716         }
 717
 718         return false;
 719 }
 720
 721
 722 int main(int argc, char** argv)
 723 {
 724         DebugOut::setDebugThreshhold(7);
 725         GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());
 726         plugin.test();
 727
 728         return 1;
 729 }