From 8a6eac36b3b1f6bae58947425bb3a25fb94571f0 Mon Sep 17 00:00:00 2001 From: Michael Carpenter Date: Tue, 13 Nov 2012 21:20:34 -0500 Subject: [PATCH] Continuation of Kevron's refactoring, including connect on first subscribe --- examples/obdsourceconfig | 2 +- plugins/obd2plugin/CMakeLists.txt | 4 +- plugins/obd2plugin/obd2source.cpp | 154 +++++++++++++++++++------------------- plugins/obd2plugin/obd2source.h | 49 +++++++++++- plugins/obd2plugin/obdpid.h | 104 ++++++++++++++++++------- 5 files changed, 204 insertions(+), 109 deletions(-) diff --git a/examples/obdsourceconfig b/examples/obdsourceconfig index f7403bd..651f776 100644 --- a/examples/obdsourceconfig +++ b/examples/obdsourceconfig @@ -3,7 +3,7 @@ { "name" : "OBD2Source", "path" : "/usr/lib/automotive-message-broker/obd2sourceplugin.so", - "device" : "/dev/ttyUSB0", + "device" : "/dev/pts/10", "baud" : "115200", "bluetoothAdapter" : "" } diff --git a/plugins/obd2plugin/CMakeLists.txt b/plugins/obd2plugin/CMakeLists.txt index 2f2bfc1..5cb6958 100644 --- a/plugins/obd2plugin/CMakeLists.txt +++ b/plugins/obd2plugin/CMakeLists.txt @@ -1,4 +1,4 @@ -if(obd2_plugin) +#if(obd2_plugin) include(CheckIncludeFiles) @@ -17,4 +17,4 @@ target_link_libraries(obd2sourceplugin amb -L${CMAKE_CURRENT_BINARY_DIR}/lib ${l install(TARGETS obd2sourceplugin LIBRARY DESTINATION lib/automotive-message-broker) -endif(obd2_plugin) +#endif(obd2_plugin) diff --git a/plugins/obd2plugin/obd2source.cpp b/plugins/obd2plugin/obd2source.cpp index b01c779..c46c3ac 100644 --- a/plugins/obd2plugin/obd2source.cpp +++ b/plugins/obd2plugin/obd2source.cpp @@ -32,7 +32,7 @@ #define __SMALLFILE__ std::string(__FILE__).substr(std::string(__FILE__).rfind("/")+1) AbstractRoutingEngine *m_re; -std::list Obd2Amb::supportedPidsList; +//std::list Obd2Amb::supportedPidsList; Obd2Amb *obd2AmbInstance = new Obd2Amb; int calledPersecond = 0; @@ -62,9 +62,11 @@ void connect(obdLib* obd, std::string device, std::string strbaud) { //printf("First: %s\nSecond: %s\n",req->arg.substr(0,req->arg.find(':')).c_str(),req->arg.substr(req->arg.find(':')+1).c_str()); std::string port = device; + DebugOut() << "Obd2Source::Connect()" << device << strbaud << "\n"; int baud = boost::lexical_cast(strbaud); obd->openPort(port.c_str(),baud); - +ObdPid::ByteArray replyVector; +std::string reply; obd->sendObdRequestString("ATZ\r",4,&replyVector,500,3); for (unsigned int i=0;i repeatReqList; ObdPid::ByteArray replyVector; std::string reply; + std::string port; + std::string baud; + bool connected=false; while (true) { //gpointer query = g_async_queue_pop(privCommandQueue); @@ -136,19 +141,31 @@ void threadLoop(gpointer data) { ObdPid *req = (ObdPid*)query; - DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got subscription request for "<req<req<req] = req->arg; //printf("Command: %s\n",req->req.c_str()); - DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->property << endl; - if (req->property == "connect") + DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->req << endl; + if (req->req == "connect") { - connect(obd); + connect(obd,req->arglist[0],req->arglist[1]); + connected = true; + } + else if (req->req == "setportandbaud") + { + port = req->arglist[0]; + baud = req->arglist[1]; + } + else if (req->req == "disconnect") + { + obd->closePort(); + connected = false; } delete req; } @@ -156,27 +173,50 @@ void threadLoop(gpointer data) if (query != nullptr) { DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got unsubscription request\n"; - ObdRequest *req = (ObdRequest*)query; - for (std::list::iterator i=reqList.begin();i!= reqList.end();i++) + ObdPid *req = (ObdPid*)query; + for (std::list::iterator i=reqList.begin();i!= reqList.end();i++) { - if ((*i) == req->req) + if ((*i)->property == req->property) { reqList.erase(i); + delete (*i); i--; } } //reqList.push_back(req->req); delete req; } - - for (std::list::iterator i=reqList.begin();i!= reqList.end();i++) + if (reqList.size() > 0 && !connected) + { + CommandRequest *req = new CommandRequest(); + req->req = "connect"; + req->arglist.push_back(port); + req->arglist.push_back(baud); + g_async_queue_push(privCommandQueue,req); + continue; + } + for (std::list::iterator i=reqList.begin();i!= reqList.end();i++) { repeatReqList.push_back(*i); } - for (std::list::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++) + for (std::list::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++) { + if (!obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector)) + { + DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unable to send request:" << (*i)->pid << "!\n"; + continue; + } + //ObdPid *pid = ObdPid::pidFromReply(replyVector); + ObdPid *pid = obd2AmbInstance->createPidFromReply(replyVector); + if (!pid) + { + //Invalid reply + DebugOut() << "Invalid reply\n"; + continue; + } + g_async_queue_push(privResponseQueue,pid); //printf("Req: %s\n",(*i).c_str()); - if ((*i) == "ATRV\r") + /*if ((*i) == "ATRV\r") { //printf("Requesting voltage...\n"); if (!obd->sendObdRequestString((*i).c_str(),(*i).length(),&replyVector)) @@ -191,73 +231,29 @@ void threadLoop(gpointer data) replystring += replyVector[j]; } //printf("Voltage reply: %s\n",replystring.c_str()); - replystring.substr(0,replystring.find("V")); - ObdReply *rep = new ObdReply(); + replystring.substr(0,replystring.find("V"));*/ + /*ObdReply *rep = new ObdReply(); rep->req = "ATRV\r"; rep->reply = replystring; - g_async_queue_push(privResponseQueue,rep); - } + g_async_queue_push(privResponseQueue,rep);*/ + /*} if (!obd->sendObdRequest((*i).c_str(),(*i).length(),&replyVector)) { //printf("Error sending obd2 request\n"); DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error sending OBD2 request\n"; continue; - } + }*/ //printf("Reply: %i %i\n",replyVector[0],replyVector[1]); - if (replyVector[0] == 0x41) - { - if (replyVector[1] == 0x0C) - { - double rpm = ((replyVector[2] << 8) + replyVector[3]) / 4.0; - ObdReply *rep = new ObdReply(); - rep->req = "0C"; - rep->property = VehicleProperty::EngineSpeed; - rep->reply = boost::lexical_cast(rpm); - g_async_queue_push(privResponseQueue,rep); - //printf("RPM: %f\n",rpm); - } - else if (replyVector[1] == 0x0D) - { - int mph = replyVector[2]; - ObdReply *rep = new ObdReply(); - rep->req = "0D"; - rep->property = VehicleProperty::VehicleSpeed; - rep->reply = boost::lexical_cast(mph); - g_async_queue_push(privResponseQueue,rep); - } - else if (replyVector[1] == 0x05) - { - int temp = replyVector[2] - 40; - ObdReply *rep = new ObdReply(); - rep->req = "05"; - rep->property = VehicleProperty::EngineCoolantTemperature; - rep->reply = boost::lexical_cast(temp); - g_async_queue_push(privResponseQueue,rep); - } - else if (replyVector[1] == 0x10) - { - double maf = ((replyVector[2] << 8) + replyVector[3]) / 100.0; - ObdReply *rep = new ObdReply(); - rep->req = "10"; - rep->property = VehicleProperty::MassAirFlow; - rep->reply = boost::lexical_cast(maf); - g_async_queue_push(privResponseQueue,rep); - } - else - { - //printf("Unknown response type: %i\n",replyVector[1]); - DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unknown response type" << replyVector[1] << endl; - } - } + /* + /* else if (replyVector[0] == 0x49) { - /* + /* 49 02 01 00 00 00 31 49 02 02 47 31 4A 43 49 02 03 35 34 34 34 49 02 04 52 37 32 35 49 02 05 32 33 36 37 - */ //VIN number reply string vinstring; for (int j=0;jreq = "0902"; rep->reply = vinstring; - g_async_queue_push(privResponseQueue,rep); - //printf("VIN Number: %i %s\n",replyVector.size(),vinstring.c_str()); + g_async_queue_push(privResponseQueue,rep);*/ - } - //DebugOut()<<"Reply: "< config) //printf("OBD2Source::setConfiguration\n"); for (map::iterator i=configuration.begin();i!=configuration.end();i++) { - //printf("Incoming setting: %s:%s\n",(*i).first.c_str(),(*i).second.c_str()); + printf("Incoming setting: %s:%s\n",(*i).first.c_str(),(*i).second.c_str()); DebugOut(5) << __SMALLFILE__ <<":"<< __LINE__ << "Incoming setting:" << (*i).first << ":" << (*i).second << "\n"; if ((*i).first == "device") { @@ -442,7 +436,15 @@ void OBD2Source::setConfiguration(map config) else throw std::runtime_error("Device Error"); } - connect(obd, port, baud); + //connect(obd, port, baud); + CommandRequest *req = new CommandRequest(); + req->req = "setportandbaud"; + req->arglist.push_back(port); + req->arglist.push_back(baud); + g_async_queue_push(commandQueue,req); + + m_port = port; + m_baud = baud; g_thread_new("mythread",(GThreadFunc)&threadLoop,this); g_idle_add(updateProperties, this); } @@ -568,7 +570,7 @@ void OBD2Source::subscribeToPropertyChanges(VehicleProperty::Property property) } - ObdPid *pid = Obd2Amb::createPidforProperty(property); + ObdPid *pid = obd2AmbInstance->createPidforProperty(property); g_async_queue_push(subscriptionAddQueue,pid); } } @@ -639,7 +641,7 @@ void OBD2Source::unsubscribeToPropertyChanges(VehicleProperty::Property property return; } - ObdPid *pid = Obd2Amb::createPidforProperty(property); + ObdPid *pid = obd2AmbInstance->createPidforProperty(property); g_async_queue_push(subscriptionRemoveQueue,pid); } @@ -714,7 +716,7 @@ void OBD2Source::getPropertyAsync(AsyncPropertyReply *reply) return; } - ObdPid* requ = Obd2Amb::createPidforProperty(property); + ObdPid* requ = obd2AmbInstance->createPidforProperty(property); g_async_queue_push(singleShotQueue,requ); } diff --git a/plugins/obd2plugin/obd2source.h b/plugins/obd2plugin/obd2source.h index 482be63..a37b694 100644 --- a/plugins/obd2plugin/obd2source.h +++ b/plugins/obd2plugin/obd2source.h @@ -35,13 +35,39 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA #include "obdpid.h" +class ObdRequest +{ +public: + VehicleProperty::Property property; + std::string req; + std::string arg; +}; + + +class CommandRequest +{ +public: + std::string req; + std::vector arglist; +}; + +class ObdReply +{ +public: + VehicleProperty::Property property; + std::string req; + std::string reply; +}; + + + class Obd2Amb { public: typedef function ConversionFunction; - + typedef std::vector ByteArray; Obd2Amb() { supportedPidsList.push_back(new VehicleSpeedPid()); @@ -52,8 +78,22 @@ public: supportedPidsList.push_back(new FuelConsumptionPid()); supportedPidsList.push_back(new EngineCoolantPid()); } - - static ObdPid* createPidforProperty(VehicleProperty::Property property) + ObdPid* createPidFromReply(ByteArray replyVector) + { + for(auto itr = supportedPidsList.begin(); itr != supportedPidsList.end(); itr++) + { + if (!(*itr)->tryParse(replyVector)) + { + continue; + } + + ObdPid* pid = (*itr)->create(); + pid->tryParse(replyVector); + return pid; + } + return 0; + } + ObdPid* createPidforProperty(VehicleProperty::Property property) { for(auto itr = supportedPidsList.begin(); itr != supportedPidsList.end(); itr++) { @@ -65,7 +105,7 @@ public: } } - static std::list supportedPidsList; + std::list supportedPidsList; }; class OBD2Source : public AbstractSource @@ -100,6 +140,7 @@ public: void setConfiguration(map config); //void randomizeProperties(); std::string m_port; + std::string m_baud; map propertyReplyMap; void updateProperty(VehicleProperty::Property property,AbstractPropertyType *value); obdLib * obd; diff --git a/plugins/obd2plugin/obdpid.h b/plugins/obd2plugin/obdpid.h index b21c81f..6632672 100644 --- a/plugins/obd2plugin/obdpid.h +++ b/plugins/obd2plugin/obdpid.h @@ -4,6 +4,7 @@ #include #include #include +#include "obdlib.h" #include class ObdPid @@ -16,10 +17,31 @@ public: { } - + static ByteArray cleanup(ByteArray replyVector) + { + ByteArray tmp; + for (int i=0;i(mph); + return true; } }; @@ -72,11 +104,16 @@ public: { } - - void parse(ByteArray replyVector) + bool tryParse(ByteArray replyVector) { - double rpm = ((replyVector[2] << 8) + replyVector[3]) / 4.0; + ByteArray tmp = compress(cleanup(replyVector)); + if (tmp[1] != 0x0C) + { + return false; + } + double rpm = ((tmp[2] << 8) + tmp[3]) / 4.0; value = boost::lexical_cast(rpm); + return false; } }; @@ -89,11 +126,16 @@ public: { } - - void parse(ByteArray replyVector) + bool tryParse(ByteArray replyVector) { - int temp = replyVector[2] - 40; + ByteArray tmp = compress(cleanup(replyVector)); + if (tmp[1] != 0x05) + { + return false; + } + int temp = tmp[2] - 40; value = boost::lexical_cast(temp); + return false; } }; @@ -106,11 +148,16 @@ public: { } - - virtual void parse(ByteArray replyVector) + bool tryParse(ByteArray replyVector) { - maf = ((replyVector[2] << 8) + replyVector[3]) / 100.0; + ByteArray tmp = compress(cleanup(replyVector)); + if (tmp[1] != 0x10) + { + return false; + } + maf = ((tmp[2] << 8) + tmp[3]) / 100.0; value = boost::lexical_cast(maf); + return false; } protected: @@ -126,10 +173,9 @@ public: { } - - virtual void parse(ByteArray replyVector) + bool tryParse(ByteArray replyVector) { - MassAirFlowPid::parse(replyVector); + MassAirFlowPid::tryParse(replyVector); timespec t; clock_gettime(CLOCK_REALTIME, &t); @@ -141,6 +187,7 @@ public: double consumption = 1 / (14.75 * 6.26) * maf * diffTime/60; value = boost::lexical_cast(consumption); + return false; } private: @@ -158,25 +205,30 @@ public: { type = 0x49; } - - virtual void parse(ByteArray replyVector) + bool tryParse(ByteArray replyVector) { std::string vinstring; - for (int j=0;j