From 899cebf073da1be73e65888ed911a33154a64f52 Mon Sep 17 00:00:00 2001 From: Kevron Rees Date: Thu, 29 Aug 2013 17:02:29 -0700 Subject: [PATCH] initial gpsnmea plugin --- CMakeLists.txt | 1 + examples/gpsnmea | 14 ++ plugins/CMakeLists.txt | 1 + plugins/gpsnmea/CMakeLists.txt | 12 ++ plugins/gpsnmea/gpsnmea.cpp | 284 +++++++++++++++++++++++++++++++++++++++++ plugins/gpsnmea/gpsnmea.h | 64 ++++++++++ 6 files changed, 376 insertions(+) create mode 100644 examples/gpsnmea create mode 100644 plugins/gpsnmea/CMakeLists.txt create mode 100644 plugins/gpsnmea/gpsnmea.cpp create mode 100644 plugins/gpsnmea/gpsnmea.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7074371..08ccbb8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ option(gpsd_plugin "gpsd location plugin" OFF) option(murphy_plugin "murphy policy framework plugin" OFF) option(test_plugin "Test Plugin" ON) option(bluemonkey_plugin "bluemonkey irc plugin" OFF) +option(gpsnmea_plugin "gps NMEA location plugin" OFF) if(opencvlux_plugin) message(STATUS "OpenCV Lux plugin enabled") diff --git a/examples/gpsnmea b/examples/gpsnmea new file mode 100644 index 0000000..79be525 --- /dev/null +++ b/examples/gpsnmea @@ -0,0 +1,14 @@ +{ + "sources" : [ + { + "name" : "gps nmea plugin", + "path" : "/usr/lib/automotive-message-broker/gpsnmea.so" + } + ], + "sinks": [ + { + "path" : "/usr/lib/automotive-message-broker/dbussinkplugin.so" + } + ] +} + diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 1c16960..1b78efd 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -32,3 +32,4 @@ add_subdirectory(gpsd) add_subdirectory(murphyplugin) add_subdirectory(testplugin) add_subdirectory(bluemonkey) +add_subdirectory(gpsnmea) diff --git a/plugins/gpsnmea/CMakeLists.txt b/plugins/gpsnmea/CMakeLists.txt new file mode 100644 index 0000000..908bf03 --- /dev/null +++ b/plugins/gpsnmea/CMakeLists.txt @@ -0,0 +1,12 @@ +if(gpsnmea_plugin) + +set(gpsnmea_headers gpsnmea.h) +set(gpsnmea_sources gpsnmea.cpp) + +add_library(gpsnmea MODULE ${gpsnmea_sources}) +set_target_properties(gpsnmea PROPERTIES PREFIX "") +target_link_libraries(gpsnmea amb -L${CMAKE_CURRENT_BINARY_DIR}/lib ${link_libraries} boost_regex) + +install(TARGETS gpsnmea LIBRARY DESTINATION lib/automotive-message-broker) + +endif(gpsnmea_plugin) diff --git a/plugins/gpsnmea/gpsnmea.cpp b/plugins/gpsnmea/gpsnmea.cpp new file mode 100644 index 0000000..9d59080 --- /dev/null +++ b/plugins/gpsnmea/gpsnmea.cpp @@ -0,0 +1,284 @@ +/* +Copyright (C) 2012 Intel Corporation + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include "gpsnmea.h" +#include "timestamp.h" + +#include +#include +#include +#include + +using namespace std; + +#include "debugout.h" +#include "abstractpropertytype.h" + +std::string gprmcRegEx = "[\\$]?GPRMC,([0-1][0-9]|2[0-3])([0-5][0-9])([0-5][0-9])," /** time hh mm ss **/ + "([AV])," /** Status A= Active, V= Void **/ + "([0-8][0-9]|90)([0-5][0-9]\\.[0-9]{4})," /** latitude **/ + "([NS])," /** lat North or South **/ + "(180|0[0-9]{2}|1[0-7][0-9])([0-5][0-9]\\.[0-9]{4})," /** longitude **/ + "([EW])," /** lon E or W **/ + "([0-9]{3}\\.[0-9])," /** Speed in knots **/ + "(3[0-5][0-9]|[0-2][0-9]{2}\\.[0-9])," /** Direction **/ + "(3[0-1]|[1-2][0-9]|0[1-9])(0[1-9]|1[0-2])([0-9]{2})," + "(180|[1][0-7][0-9]|[0][0-9]{2}\\.[0-9])," /** Magnetic variation **/ + "([EW])" /** Magnetic Direction **/ + "\\*([0-9,A-F]{2})"; + +class Location +{ +public: + Location(); + + void parse(std::string gprmc); + + VehicleProperty::LatitudeType latitude() + { + return mLatitude; + } + + VehicleProperty::LongitudeType longitude() + { + return mLongitude; + } + + VehicleProperty::AltitudeType altitude() + { + return mAltitude; + } + + VehicleProperty::DirectionType direction() + { + return mDirection; + } + + VehicleProperty::VehicleSpeedType speed() + { + return mSpeed; + } + + BasicPropertyType gpsTime() + { + return mGpsTime; + } + +private: ///methods: + + void parseGprmc(string gprmc); + + void parseTime(std::string h, std::string m, std::string s); + void parseLatitude(std::string d, std::string m, std::string ns); + void parseLongitude(std::string d, string m, string ew); + void parseSpeed(std::string spd); + void parseDirection(std::string dir); + void parseAltitude(std::string alt); + + double degsToDecimal(double degs); + +private: + + VehicleProperty::LatitudeType mLatitude; + VehicleProperty::LongitudeType mLongitude; + VehicleProperty::AltitudeType mAltitude; + VehicleProperty::DirectionType mDirection; + VehicleProperty::VehicleSpeedType mSpeed; + BasicPropertyType mGpsTime; + + boost::regex regularExpression; + bool isActive; + +}; + +Location::Location() + :mGpsTime("GpsTime",0), isActive(false) +{ + +} + +void Location::parse(string nmea) +{ + if(boost::algorithm::starts_with(nmea,"$GPRMC")) + { + parseGprmc(nmea); + } +} + +void Location::parseGprmc(string gprmc) +{ + regularExpression.assign(gprmcRegEx); + + boost::smatch tokens; + + if (boost::regex_match (gprmc, tokens, regularExpression) ) + { + parseTime(tokens[1],tokens[2],tokens[3]); + + if(tokens[4] == "A") + { + isActive = true; + } + + parseLatitude(tokens[5], tokens[6], tokens[7]); + parseLongitude(tokens[8], tokens[9], tokens[10]); + parseSpeed(tokens[11]); + parseDirection(tokens[12]); + } +} + +void Location::parseTime(string h, string m, string s) +{ + tm t; + t.tm_hour = boost::lexical_cast(h); + t.tm_min = boost::lexical_cast(m); + t.tm_sec = boost::lexical_cast(s); + + time_t time = mktime(&t); + + mGpsTime.setValue((double)time); +} + +void Location::parseLatitude(string d, string m, string ns) +{ + double degs = boost::lexical_cast(d + m); + double dec = degsToDecimal(degs); + + if(ns == "S") + dec *= -1; + + mLatitude.setValue(dec); +} + +void Location::parseLongitude(string d, string m, string ew) +{ + double degs = boost::lexical_cast(d + m); + double dec = degsToDecimal(degs); + + if(ew == "E") + dec *= -1; + + mLongitude.setValue(dec); +} + +void Location::parseSpeed(string spd) +{ + double s = boost::lexical_cast(spd); + + ///to kph: + s *= 1.852; + + mSpeed = VehicleProperty::VehicleSpeedType(s); +} + +void Location::parseDirection(string dir) +{ + uint16_t d = boost::lexical_cast(dir); + mDirection = VehicleProperty::DirectionType(d); +} + +void Location::parseAltitude(string alt) +{ + +} + +double Location::degsToDecimal(double degs) +{ + double deg; + double min = 100.0 * modf(degs / 100.0, °); + return deg + (min / 60.0); +} + +extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map config) +{ + return new GpsNmeaSource(routingengine, config); + +} + +GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map config) + :AbstractSource(re,config) +{ + addPropertySupport(VehicleProperty::Latitude, Zone::None); + addPropertySupport(VehicleProperty::Longitude, Zone::None); + //addPropertySupport(VehicleProperty::Altitude, Zone::None); + addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None); + addPropertySupport(VehicleProperty::Direction, Zone::None); + + + ///test: + + Location location; + location.parse("$GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E"); + + DebugOut()<property< zones; + + zones.push_back(zone); + + PropertyInfo info(0, zones); + + propertyInfoMap[property] = info; +} diff --git a/plugins/gpsnmea/gpsnmea.h b/plugins/gpsnmea/gpsnmea.h new file mode 100644 index 0000000..352257b --- /dev/null +++ b/plugins/gpsnmea/gpsnmea.h @@ -0,0 +1,64 @@ +/* +Copyright (C) 2012 Intel Corporation + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef GPSNMEAPLUGIN_H +#define GPSNMEAPLUGIN_H + +#include +#include + +using namespace std; + +class GpsNmeaSource: public AbstractSource +{ + +public: + GpsNmeaSource(AbstractRoutingEngine* re, map config); + + string uuid(); + void getPropertyAsync(AsyncPropertyReply *reply); + void getRangePropertyAsync(AsyncRangePropertyReply *reply); + AsyncPropertyReply * setProperty(AsyncSetPropertyRequest request); + void subscribeToPropertyChanges(VehicleProperty::Property property); + void unsubscribeToPropertyChanges(VehicleProperty::Property property); + PropertyList supported(); + + int supportedOperations(); + + void propertyChanged(VehicleProperty::Property property, AbstractPropertyType* value, string uuid) {} + void supportedChanged(PropertyList) {} + + PropertyInfo getPropertyInfo(VehicleProperty::Property property) + { + if(propertyInfoMap.find(property) != propertyInfoMap.end()) + return propertyInfoMap[property]; + + return PropertyInfo::invalid(); + } + +private: + + void addPropertySupport(VehicleProperty::Property property, Zone::Type zone); + + std::map propertyInfoMap; + + PropertyList mRequests; + PropertyList mSupported; +}; + +#endif // EXAMPLEPLUGIN_H -- 2.7.4