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