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 "obd2source.h"
22 #include <boost/assert.hpp>
23 #include <boost/lexical_cast.hpp>
26 //#include <json-glib/json-glib.h>
27 #include <listplusplus.h>
29 #include "bluetooth.hpp"
30 #include "timestamp.h"
31 #include "bluetooth5.h"
32 #include "superptr.hpp"
38 #define __SMALLFILE__ std::string(__FILE__).substr(std::string(__FILE__).rfind("/")+1)
39 AbstractRoutingEngine *m_re;
41 //std::list<ObdPid*> Obd2Amb::supportedPidsList;
42 Obd2Amb *obd2AmbInstance = new Obd2Amb;
43 VehicleProperty::Property Obd2Connected = "Obd2Connected";
44 int calledPersecond = 0;
46 bool sendElmCommand(obdLib *obd,std::string command)
48 std::vector<unsigned char> replyVector;
50 obd->sendObdRequestString(command.append("\r").c_str(),command.length()+1,&replyVector,10,3);
51 for (unsigned int i=0;i<replyVector.size();i++)
53 reply += replyVector[i];
55 if (reply.find("OK") == -1)
67 bool beginsWith(std::string a, std::string b)
69 return (a.compare(0, b.length(), b) == 0);
72 bool connect(obdLib* obd, std::string device, std::string strbaud, int fd = -1)
74 //printf("First: %s\nSecond: %s\n",req->arg.substr(0,req->arg.find(':')).c_str(),req->arg.substr(req->arg.find(':')+1).c_str());
75 std::string port = device;
76 DebugOut() << "Obd2Source::Connect()" << device << strbaud << endl;
77 int baud = boost::lexical_cast<int>(strbaud);
81 if(obd->openPort(fd, baud) == -1)
86 if(obd->openPort(port.c_str(),baud) == -1)
90 ObdPid::ByteArray replyVector;
92 obd->sendObdRequestString("ATZ\r",4,&replyVector,500,3);
93 for (unsigned int i=0;i<replyVector.size();i++)
95 reply += replyVector[i];
97 if (reply.find("ELM") == -1)
100 //printf("Error!\n");
101 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error resetting ELM"<<endl;
106 //printf("Reply to reset: %s\n",reply.c_str());
108 if (!sendElmCommand(obd,"ATSP0"))
110 //printf("Error sending echo\n");
111 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error setting auto protocol"<<endl;
114 if (!sendElmCommand(obd,"ATE0"))
116 //printf("Error sending echo\n");
117 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error turning off echo"<<endl;
120 if (!sendElmCommand(obd,"ATH0"))
122 //printf("Error sending headers off\n");
123 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error turning off headers"<<endl;
126 if (!sendElmCommand(obd,"ATL0"))
128 //printf("Error turning linefeeds off\n");
129 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error turning off linefeeds"<<endl;
132 obd->sendObdRequestString("010C1\r", 6, &replyVector, 500, 5);
137 void threadLoop(gpointer data)
139 GAsyncQueue *privCommandQueue = g_async_queue_ref(((OBD2Source*)data)->commandQueue);
140 GAsyncQueue *privResponseQueue = g_async_queue_ref(((OBD2Source*)data)->responseQueue);
141 GAsyncQueue *privSingleShotQueue = g_async_queue_ref(((OBD2Source*)data)->singleShotQueue);
142 GAsyncQueue *privSubscriptionAddQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionAddQueue);
143 GAsyncQueue *privSubscriptionRemoveQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionRemoveQueue);
144 GAsyncQueue *privStatusQueue = g_async_queue_ref(((OBD2Source*)data)->statusQueue);
146 obdLib *obd = new obdLib();
147 OBD2Source *source = (OBD2Source*)data;
149 obd->setCommsCallback([](const char* mssg, void* data) { DebugOut(6)<<mssg<<endl; },NULL);
150 obd->setDebugCallback([](const char* mssg, void* data, obdLib::DebugLevel debugLevel) { DebugOut(debugLevel)<<mssg<<endl; },NULL);
152 std::list<ObdPid*> reqList;
153 std::list<ObdPid*> repeatReqList;
154 ObdPid::ByteArray replyVector;
157 bool connected=false;
159 int timeoutCount = 0;
160 while (source->m_threadLive)
162 gpointer query = g_async_queue_try_pop(privSingleShotQueue);
163 if (query != nullptr)
165 //printf("Got request!\n");
167 ObdPid *req = (ObdPid*)query;
168 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got single shot request: " << req->pid.substr(0,req->pid.length()-1) << ":" << req->property <<endl;
169 repeatReqList.push_back(req);
171 query = g_async_queue_try_pop(privSubscriptionAddQueue);
172 if (query != nullptr)
175 ObdPid *req = (ObdPid*)query;
176 //DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got subscription request for "<<req->req<<endl;
177 reqList.push_back(req);
179 query = g_async_queue_try_pop(privCommandQueue);
180 if (query != nullptr)
182 //ObdPid *req = (ObdPid*)query;
183 CommandRequest *req = (CommandRequest*)query;
184 //commandMap[req->req] = req->arg;
185 //printf("Command: %s\n",req->req.c_str());
186 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->req << endl;
187 if (req->req == "connectifnot")
191 if (source->m_isBluetooth)
194 bt.getDeviceForAddress(source->m_btDeviceAddress, [&obd, baud, &privStatusQueue, &connected](int fd)
196 connected = connect(obd, "", baud, fd);
200 StatusMessage *statusreq = new StatusMessage();
201 statusreq->statusStr = "connected";
202 g_async_queue_push(privStatusQueue, statusreq);
206 StatusMessage *statusreq = new StatusMessage();
207 statusreq->statusStr = "disconnected";
208 g_async_queue_push(privStatusQueue, statusreq);
213 std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress);
216 DebugOut(3) << "Using bluetooth device \"" << source->m_btDeviceAddress << "\" bound to: " << tempPort << endl;
221 DebugOut(DebugOut::Error) << "Error creating bluetooth device" << endl;
225 connected = connect(obd, port, baud);
229 StatusMessage *statusreq = new StatusMessage();
230 statusreq->statusStr = "connected";
231 g_async_queue_push(privStatusQueue, statusreq);
235 StatusMessage *statusreq = new StatusMessage();
236 statusreq->statusStr = "disconnected";
237 g_async_queue_push(privStatusQueue, statusreq);
245 connected = connect(obd, port, baud);
249 StatusMessage *statusreq = new StatusMessage();
250 statusreq->statusStr = "connected";
251 g_async_queue_push(privStatusQueue, statusreq);
255 StatusMessage *statusreq = new StatusMessage();
256 statusreq->statusStr = "disconnected";
257 g_async_queue_push(privStatusQueue, statusreq);
262 else if (req->req == "setportandbaud")
264 port = req->arglist[0];
265 baud = req->arglist[1];
267 else if (req->req == "disconnect")
269 DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Using queued disconnect" << (ulong)req << endl;
275 bt.disconnect(source->m_btDeviceAddress, source->m_btAdapterAddress);
278 StatusMessage *statusreq = new StatusMessage();
279 statusreq->statusStr = "disconnected";
280 g_async_queue_push(privStatusQueue, statusreq);
284 query = g_async_queue_try_pop(privSubscriptionRemoveQueue);
285 if (query != nullptr)
287 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got unsubscription request"<<endl;
288 ObdPid *req = (ObdPid*)query;
289 for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
291 if ((*i)->property == req->property)
296 if (reqList.size() == 0)
302 //reqList.push_back(req->req);
305 if (reqList.size() > 0 && !connected)
307 /*CommandRequest *req = new CommandRequest();
308 req->req = "connect";
309 req->arglist.push_back(port);
310 req->arglist.push_back(baud);
311 g_async_queue_push(privCommandQueue,req);
314 else if (reqList.size() == 0 && connected)
317 if (emptycount < 1000)
323 CommandRequest *req = new CommandRequest();
324 req->req = "disconnect";
325 g_async_queue_push(privCommandQueue,req);
333 for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
335 repeatReqList.push_back(*i);
338 for (std::list<ObdPid*>::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++)
340 DebugOut(10) << __SMALLFILE__ << ":" << __LINE__ << "Requesting pid: " << (*i)->pid.substr(0,(*i)->pid.length()-1) << (*i)->property << endl;
341 if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
343 //Don't erase the pid, just skip over it.
344 int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
354 if(beginsWith((*i)->pid,"AT") || beginsWith((*i)->pid, "ST"))
356 result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector);
358 else result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector,5,3);
362 //This only happens during a error with the com port. Close it and re-open it later.
363 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unable to send request:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
364 if (obd->lastError() == obdLib::NODATA)
366 DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::NODATA for pid" << (*i)->pid.substr(0,(*i)->pid.length()-1) << " expected property: " << (*i)->property << endl;
367 if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
369 //pid value i not yet in the list.
370 int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
375 source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
376 source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,count));
380 source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,1));
382 StatusMessage *statusreq = new StatusMessage();
383 statusreq->statusStr = "error:nodata";
384 statusreq->property = (*i)->property;
385 g_async_queue_push(privStatusQueue,statusreq);
388 else if (obd->lastError() == obdLib::TIMEOUT)
391 if (timeoutCount < 2)
393 DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::TIMEOUT for pid" << (*i)->pid << endl;
394 StatusMessage *statusreq = new StatusMessage();
395 statusreq->statusStr = "error:timeout";
396 g_async_queue_push(privStatusQueue,statusreq);
402 DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBD Other error:" << obd->lastError() << endl;
405 CommandRequest *req = new CommandRequest();
406 DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Queuing up a disconnect" << (ulong)req << endl;
407 req->req = "disconnect";
408 g_async_queue_push(privCommandQueue,req);
409 i = repeatReqList.end();
413 if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
415 //If we get the pid response, then we want to clear out the blacklist list.
416 source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
419 //ObdPid *pid = ObdPid::pidFromReply(replyVector);
420 ObdPid *pid = obd2AmbInstance->createPidFromReply(replyVector);
424 DebugOut() << "Invalid reply"<<endl;
429 DebugOut(11) << __SMALLFILE__ <<":"<< __LINE__ << "Reply recieved and queued for:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
431 for (int i=0;i<replyVector.size();i++)
433 if (replyVector[i] != 13)
435 repstr += (char)replyVector[i];
437 //DebugOut(11) << replyVector[i];
439 DebugOut(11) << "Reply:" << repstr << endl;
441 g_async_queue_push(privResponseQueue,pid);
445 //We had zero non-blacklisted events. Pause for a moment here to keep from burning CPU.
448 repeatReqList.clear();
456 static int updateProperties( gpointer data)
459 OBD2Source* src = (OBD2Source*)data;
461 while (gpointer retval = g_async_queue_try_pop(src->statusQueue))
463 StatusMessage *reply = (StatusMessage*)retval;
464 if (reply->statusStr == "disconnected")
466 src->obd2Connected.setValue(false);
467 src->updateProperty(&src->obd2Connected);
469 else if (reply->statusStr == "connected")
471 src->obd2Connected.setValue(true);
472 src->updateProperty(&src->obd2Connected);
474 else if (reply->statusStr == "error:nodata" || reply->statusStr == "error:timeout")
476 AsyncPropertyReply* srcReply = nullptr;
478 for(auto i : src->propertyReplyList)
480 if(i->property == reply->property)
489 DebugOut(5) << __SMALLFILE__ <<":"<< __LINE__ << reply->statusStr << " on property:" << reply->property << endl;
490 srcReply->success = false;
491 srcReply->error = AsyncPropertyReply::InvalidOperation;
492 srcReply->completed(srcReply);
493 removeOne(&src->propertyReplyList, srcReply);
495 /// Remove support for this pid:
496 PropertyList list = src->supported();
497 removeOne(&list, reply->property);
498 src->setSupported(list);
502 DebugOut(5) << __SMALLFILE__ <<":"<< __LINE__ << reply->statusStr << " on unrequested property:" << reply->property << endl;
507 while(gpointer retval = g_async_queue_try_pop(src->responseQueue))
509 ObdPid *reply = (ObdPid*)retval;
511 auto value = amb::make_unique(VehicleProperty::getPropertyTypeForPropertyNameValue(reply->property, reply->value));
512 value->priority = AbstractPropertyType::Instant;
513 src->updateProperty(value.get());
519 void OBD2Source::updateProperty(AbstractPropertyType* value)
521 VehicleProperty::Property property = value->name;
522 AsyncPropertyReply* reply = nullptr;
524 DebugOut() << "updateProperty for: " << property << "value: " << value->toString() << endl;
526 for(auto i : propertyReplyList)
528 if(i->property == property)
538 reply->value = value;
539 reply->success = true;
541 reply->completed(reply);
544 DebugOut(DebugOut::Error)<<"failed to call reply completed callback"<<endl;
547 removeOne(&propertyReplyList, reply);
550 if(oldValueMap.find(property) != oldValueMap.end())
552 AbstractPropertyType* old = oldValueMap[property];
554 if((*old) == (*value))
556 DebugOut() << "old value is same as new for: " << value->name << endl;
561 oldValueMap.erase(property);
565 oldValueMap[property] = value->copy();
568 DebugOut() << "updateProperty for: " << property << "value: " << value->toString() << endl;
569 m_re->updateProperty(value, uuid());
573 void OBD2Source::setSupported(PropertyList list)
575 m_supportedProperties = list;
576 m_re->updateSupported(list,PropertyList(),this);
578 /*void OBD2Source::propertySignal(VehicleProperty::Property property,boost::any value)
581 void OBD2Source::checkProperty()
584 void OBD2Source::setConfiguration(map<string, string> config)
586 // //Config has been passed, let's start stuff up.
587 configuration = config;
590 std::string port = "/dev/ttyUSB0";
591 std::string baud = "115200";
592 std::string btadapter = "";
593 m_isBluetooth = false;
596 //printf("OBD2Source::setConfiguration\n");
597 for (map<string,string>::iterator i=configuration.begin();i!=configuration.end();i++)
599 //printf("Incoming setting: %s:%s\n",(*i).first.c_str(),(*i).second.c_str());
600 DebugOut(5) << __SMALLFILE__ <<":"<< __LINE__ << "Incoming setting:" << (*i).first << ":" << (*i).second << endl;
601 if ((*i).first == "device")
605 else if ((*i).first == "baud")
607 if((*i).second != "")
611 else if ((*i).first == "bluetoothAdapter")
613 btadapter = (*i).second;
617 if(port.find(":") != string::npos)
619 m_btDeviceAddress = port;
620 m_btAdapterAddress = btadapter;
621 m_isBluetooth = true;
624 //connect(obd, port, baud);
625 CommandRequest *req = new CommandRequest();
626 req->req = "setportandbaud";
627 req->arglist.push_back(port);
628 req->arglist.push_back(baud);
629 g_async_queue_push(commandQueue, req);
633 m_gThread = g_thread_new("mythread", (GThreadFunc)&threadLoop, this);
634 g_timeout_add(5, updateProperties, this);
637 OBD2Source::OBD2Source(AbstractRoutingEngine *re, map<string, string> config)
638 : AbstractSource(re, config), obd2Connected(Obd2Connected,false)
640 bool success = VehicleProperty::registerProperty(Obd2Connected, [](){ return new Obd2ConnectType(Obd2Connected, false); });
647 clientConnected = false;
654 for(auto itr = obd2amb.supportedPidsList.begin(); itr != obd2amb.supportedPidsList.end(); itr++)
656 m_supportedProperties.push_back((*itr)->property);
659 m_supportedProperties.push_back(Obd2Connected);
661 re->updateSupported(supported(), PropertyList(), this);
663 statusQueue = g_async_queue_new();
664 commandQueue = g_async_queue_new();
665 subscriptionAddQueue = g_async_queue_new();
666 subscriptionRemoveQueue = g_async_queue_new();
667 responseQueue = g_async_queue_new();
668 singleShotQueue = g_async_queue_new();
670 setConfiguration(config);
672 OBD2Source::~OBD2Source()
674 DebugOut() << "OBD2Source Destructor called!!!"<<endl;
675 m_threadLive = false;
676 g_thread_join(m_gThread);
679 PropertyList OBD2Source::supported()
681 return m_supportedProperties;
684 PropertyInfo OBD2Source::getPropertyInfo(const VehicleProperty::Property &)
686 Zone::ZoneList zones;
689 return PropertyInfo(0, zones);
692 int OBD2Source::supportedOperations()
697 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
699 return new OBD2Source(routingengine, config);
702 const string OBD2Source::uuid()
704 return "f77af740-f1f8-11e1-aff1-0800200c9a66";
706 void OBD2Source::subscribeToPropertyChanges(VehicleProperty::Property property)
708 if (property == VehicleProperty::VIN)
710 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "VIN subscription requested... but there's no point!"<<endl;
712 else if (property == VehicleProperty::WMI)
714 DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "WMI subscription requested... but there's no point!"<<endl;
718 if(!contains(m_supportedProperties, property))
720 DebugOut(0)<<"obd plugin does not support: "<<property<<endl;
725 ObdPid *pid = obd2AmbInstance->createPidforProperty(property);
729 DebugOut(DebugOut::Warning) << "We don't support this property: " << property <<endl;
733 //If the pid is currently in the blacklist map, erase it. This allows for applications
734 //to "un-blacklist" a pid by re-subscribing to it.
735 if (m_blacklistPidCountMap.find(pid->pid) != m_blacklistPidCountMap.end())
737 m_blacklistPidCountMap.erase(m_blacklistPidCountMap.find(pid->pid));
741 g_async_queue_push(subscriptionAddQueue, pid);
742 CommandRequest *req = new CommandRequest();
743 req->req = "connectifnot";
744 g_async_queue_push(commandQueue, req);
749 void OBD2Source::unsubscribeToPropertyChanges(VehicleProperty::Property property)
751 if(!contains(m_supportedProperties, property))
753 DebugOut(0)<<"obd plugin does not support: "<<property<<endl;
757 ObdPid *pid = obd2AmbInstance->createPidforProperty(property);
758 g_async_queue_push(subscriptionRemoveQueue,pid);
762 void OBD2Source::getPropertyAsync(AsyncPropertyReply *reply)
764 DebugOut(5) << __SMALLFILE__ <<":"<< __LINE__ << "getPropertyAsync requested for " << reply->property << endl;
766 VehicleProperty::Property property = reply->property;
769 if(!contains(m_supportedProperties, property))
771 DebugOut(0)<<"obd plugin does not support: "<<property<<endl;
775 if(reply->property == Obd2Connected)
777 reply->success = true;
778 reply->value = &obd2Connected;
779 reply->completed(reply);
783 propertyReplyList.push_back(reply);
784 reply->timedout = [this](AsyncPropertyReply* reply)
786 DebugOut() << "removing "<< reply->property << " from propertyReplyList" << endl;
787 removeOne(&propertyReplyList, reply);
790 ObdPid* requ = obd2AmbInstance->createPidforProperty(property);
791 g_async_queue_push(singleShotQueue,requ);
792 CommandRequest *req = new CommandRequest();
793 req->req = "connectifnot";
794 g_async_queue_push(commandQueue,req);
797 AsyncPropertyReply *OBD2Source::setProperty(AsyncSetPropertyRequest request )
799 AsyncPropertyReply* reply = new AsyncPropertyReply (request);
801 if(request.property == Obd2Connected)
803 propertyReplyList.push_back(reply);
804 reply->success = true;
806 if(request.value->value<bool>() == true)
808 CommandRequest *req = new CommandRequest();
809 req->req = "connectifnot";
810 g_async_queue_push(commandQueue, req);
814 CommandRequest *req = new CommandRequest();
815 req->req = "disconnect";
816 g_async_queue_push(commandQueue, req);
822 reply->success = false;
825 reply->completed(reply);