already_AddRefed<DOMRequest> BluetoothAdapter::Connect(BluetoothDevice& aDevice, const Optional<short unsigned int>& aServiceUuid, ErrorResult& aRv) { nsCOMPtr<nsPIDOMWindow> win = GetOwner(); if (!win) { aRv.Throw(NS_ERROR_FAILURE); return nullptr; } nsRefPtr<DOMRequest> request = new DOMRequest(win); nsRefPtr<BluetoothVoidReplyRunnable> results = new BluetoothVoidReplyRunnable(request); nsAutoString address; aDevice.GetAddress(address); uint32_t deviceClass = aDevice.Class(); uint16_t serviceUuid = 0; if (aServiceUuid.WasPassed()) { serviceUuid = aServiceUuid.Value(); } BluetoothService* bs = BluetoothService::Get(); if (!bs) { aRv.Throw(NS_ERROR_FAILURE); return nullptr; } bs->Connect(address, deviceClass, serviceUuid, results); return request.forget(); }
already_AddRefed<DOMRequest> BluetoothAdapter::PairUnpair(bool aPair, BluetoothDevice& aDevice, ErrorResult& aRv) { nsCOMPtr<nsPIDOMWindow> win = GetOwner(); if (!win) { aRv.Throw(NS_ERROR_FAILURE); return nullptr; } nsRefPtr<DOMRequest> request = new DOMRequest(win); nsRefPtr<BluetoothVoidReplyRunnable> results = new BluetoothVoidReplyRunnable(request); nsAutoString addr; aDevice.GetAddress(addr); BluetoothService* bs = BluetoothService::Get(); if (!bs) { aRv.Throw(NS_ERROR_FAILURE); return nullptr; } nsresult rv; if (aPair) { rv = bs->CreatePairedDeviceInternal(addr, kCreatePairedDeviceTimeout, results); } else { rv = bs->RemoveDeviceInternal(addr, results); } if (NS_FAILED(rv)) { NS_WARNING("Pair/Unpair failed!"); aRv.Throw(rv); return nullptr; } return request.forget(); }
void threadLoop(gpointer data) { GAsyncQueue *privCommandQueue = g_async_queue_ref(((OBD2Source*)data)->commandQueue); GAsyncQueue *privResponseQueue = g_async_queue_ref(((OBD2Source*)data)->responseQueue); GAsyncQueue *privSingleShotQueue = g_async_queue_ref(((OBD2Source*)data)->singleShotQueue); GAsyncQueue *privSubscriptionAddQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionAddQueue); GAsyncQueue *privSubscriptionRemoveQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionRemoveQueue); GAsyncQueue *privStatusQueue = g_async_queue_ref(((OBD2Source*)data)->statusQueue); obdLib *obd = new obdLib(); OBD2Source *source = (OBD2Source*)data; obd->setCommsCallback([](const char* mssg, void* data) { DebugOut(6)<<mssg<<endl; },NULL); obd->setDebugCallback([](const char* mssg, void* data, obdLib::DebugLevel debugLevel) { DebugOut(debugLevel)<<mssg<<endl; },NULL); std::list<ObdPid*> reqList; std::list<ObdPid*> repeatReqList; ObdPid::ByteArray replyVector; std::string reply; std::string port; std::string baud; bool connected=false; int emptycount = 0; int timeoutCount = 0; while (source->m_threadLive) { //gpointer query = g_async_queue_pop(privCommandQueue); gpointer query = g_async_queue_try_pop(privSingleShotQueue); if (query != nullptr) { //printf("Got request!\n"); ObdPid *req = (ObdPid*)query; DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got single shot request: " << req->pid.substr(0,req->pid.length()-1) << ":" << req->property <<endl; repeatReqList.push_back(req); } query = g_async_queue_try_pop(privSubscriptionAddQueue); if (query != nullptr) { ObdPid *req = (ObdPid*)query; //DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got subscription request for "<<req->req<<endl; reqList.push_back(req); } query = g_async_queue_try_pop(privCommandQueue); if (query != nullptr) { //ObdPid *req = (ObdPid*)query; CommandRequest *req = (CommandRequest*)query; //commandMap[req->req] = req->arg; //printf("Command: %s\n",req->req.c_str()); DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->req << endl; if (req->req == "connect" ) { if (source->m_isBluetooth) { BluetoothDevice bt; std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress); if(tempPort != "") { DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl; port = tempPort; } } else { port = req->arglist[0]; baud = req->arglist[1]; } connected = connect(obd,port,baud); if(connected) { StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "connected"; g_async_queue_push(privStatusQueue,statusreq); } else { StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "disconnected"; g_async_queue_push(privStatusQueue,statusreq); } } else if (req->req == "connectifnot") { if (!connected) { if (source->m_isBluetooth) { BluetoothDevice bt; std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress); if(tempPort != "") { DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl; port = tempPort; } else { DebugOut(DebugOut::Error)<<"Error creating bluetooth device"<<endl; continue; } } connected = connect(obd,port,baud); if(connected) { StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "connected"; g_async_queue_push(privStatusQueue,statusreq); } else { StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "disconnected"; g_async_queue_push(privStatusQueue,statusreq); } } } else if (req->req == "setportandbaud") { port = req->arglist[0]; baud = req->arglist[1]; } else if (req->req == "disconnect") { DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Using queued disconnect" << (ulong)req << endl; obd->closePort(); BluetoothDevice bt; bt.disconnect(source->m_btDeviceAddress, source->m_btAdapterAddress); connected = false; StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "disconnected"; g_async_queue_push(privStatusQueue,statusreq); } delete req; } query = g_async_queue_try_pop(privSubscriptionRemoveQueue); if (query != nullptr) { DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got unsubscription request"<<endl; ObdPid *req = (ObdPid*)query; for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++) { if ((*i)->property == req->property) { reqList.erase(i); delete (*i); i--; if (reqList.size() == 0) { break; } } } //reqList.push_back(req->req); delete req; } 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;*/ } else if (reqList.size() == 0 && connected) { emptycount++; if (emptycount < 1000) { usleep(10000); continue; } emptycount = 0; CommandRequest *req = new CommandRequest(); req->req = "disconnect"; g_async_queue_push(privCommandQueue,req); continue; } if (!connected) { usleep(10000); continue; } for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++) { repeatReqList.push_back(*i); } int badloop = 0; for (std::list<ObdPid*>::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++) { DebugOut(10) << __SMALLFILE__ << ":" << __LINE__ << "Requesting pid: " << (*i)->pid.substr(0,(*i)->pid.length()-1) << (*i)->property << endl; if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end()) { //Don't erase the pid, just skip over it. int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second; if (count > 10) { continue; } } badloop++; bool result = false; if(beginsWith((*i)->pid,"AT") || beginsWith((*i)->pid, "ST")) { result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector); } else result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector,5,3); if (!result) { //This only happens during a error with the com port. Close it and re-open it later. DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unable to send request:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl; if (obd->lastError() == obdLib::NODATA) { DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::NODATA for pid" << (*i)->pid.substr(0,(*i)->pid.length()-1) << " expected property: " << (*i)->property << endl; if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end()) { //pid value i not yet in the list. int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second; if (count > 10) { } source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid)); source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,count)); } else { source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,1)); } StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "error:nodata"; statusreq->property = (*i)->property; g_async_queue_push(privStatusQueue,statusreq); continue; } else if (obd->lastError() == obdLib::TIMEOUT) { timeoutCount++; if (timeoutCount < 2) { DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::TIMEOUT for pid" << (*i)->pid << endl; StatusMessage *statusreq = new StatusMessage(); statusreq->statusStr = "error:timeout"; g_async_queue_push(privStatusQueue,statusreq); continue; } } else { DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBD Other error:" << obd->lastError() << endl; } CommandRequest *req = new CommandRequest(); DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Queuing up a disconnect" << (ulong)req << endl; req->req = "disconnect"; g_async_queue_push(privCommandQueue,req); i = repeatReqList.end(); i--; continue; } if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end()) { //If we get the pid response, then we want to clear out the blacklist list. source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid)); } timeoutCount = 0; //ObdPid *pid = ObdPid::pidFromReply(replyVector); ObdPid *pid = obd2AmbInstance->createPidFromReply(replyVector); if (!pid) { //Invalid reply DebugOut() << "Invalid reply"<<endl; continue; } else { DebugOut(11) << __SMALLFILE__ <<":"<< __LINE__ << "Reply recieved and queued for:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl; std::string repstr; for (int i=0;i<replyVector.size();i++) { if (replyVector[i] != 13) { repstr += (char)replyVector[i]; } //DebugOut(11) << replyVector[i]; } DebugOut(11) << "Reply:" << repstr << endl; } g_async_queue_push(privResponseQueue,pid); } if (badloop == 0) { //We had zero non-blacklisted events. Pause for a moment here to keep from burning CPU. //usleep(10000); } repeatReqList.clear(); } if (connected) { obd->closePort(); } }
GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config) :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b") { location =new Location(re, mUuid); VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); }); 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); addPropertySupport(GPSTIME, Zone::None); ///test: if(config.find("test") != config.end()) { Location location(routingEngine, mUuid); location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E"); DebugOut()<<"lat: "<<location.latitude().toString()<<endl; g_assert(location.latitude().toString() == "-23.86600833"); g_assert(location.gpsTime().toString() == "1050585131"); location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47"); DebugOut()<<"alt: "<<location.altitude().toString()<<endl; DebugOut()<<"lat: "<<location.latitude().toString()<<endl; g_assert(location.altitude().toString() == "545.4"); g_assert(location.latitude().toString() == "48.1173"); } std::string btaddapter = config["bluetoothAdapter"]; if(config.find("device")!= config.end()) { std::string dev = config["device"]; if(dev.find(":") != string::npos) { BluetoothDevice bt; dev = bt.getDeviceForAddress(dev, btaddapter); } device = new SerialPort(dev); if(!device->open()) { DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl; perror("Error"); return; } DebugOut()<<"read from device: "<<device->read()<<endl; GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor()); g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this); g_io_channel_set_close_on_unref(chan, true); g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch. } re->setSupported(supported(), this); }
OpenXCPlugin::OpenXCPlugin(AbstractRoutingEngine* re, map<string, string> config) :AbstractSource(re, config) { re->setSupported(supported(), this); /// populate the openxc to amb map: openXC2AmbMap["steering_wheel_angle"] = VehicleProperty::SteeringWheelAngle; //openXC2AmbMap["torque_at_transmission"] = VehicleProperty::Engine; openXC2AmbMap["engine_speed"] = VehicleProperty::EngineSpeed; openXC2AmbMap["vehicle_speed"] = VehicleProperty::VehicleSpeed; openXC2AmbMap["accelerator_pedal_position"] = VehicleProperty::ThrottlePosition; openXC2AmbMap["parking_brake_status"] = VehicleProperty::ParkingBrakeStatus; openXC2AmbMap["brake_pedal_status"] = VehicleProperty::LightBrake; openXC2AmbMap["transmission_gear_position"] = VehicleProperty::TransmissionGearPosition; openXC2AmbMap["odometer"] = VehicleProperty::Odometer; openXC2AmbMap["ignition_status"] = VehicleProperty::VehiclePowerMode; openXC2AmbMap["fuel_level"] = VehicleProperty::FuelLevel; openXC2AmbMap["fuel_consumed_since_restart"] = VehicleProperty::FuelConsumption; openXC2AmbMap["headlamp_status"] = VehicleProperty::LightHead; openXC2AmbMap["high_beam_status"] = VehicleProperty::LightHighBeam; openXC2AmbMap["windshield_wiper_status"] = VehicleProperty::WindshieldWiper; openXC2AmbMap["latitude"] = VehicleProperty::Latitude; openXC2AmbMap["longitude"] = VehicleProperty::Longitude; openXC2AmbMap["button_event"] = VehicleProperty::ButtonEvent; bool test = false; if(config.find("test") != config.end()) { test = config["test"] == "true"; } if(test) { testParseEngine(); } std::string bluetoothAddy = config["device"]; std::string bluetoothAdapter = config["bluetoothAdapter"]; std::string serialDevice; if(bluetoothAddy != "") { BluetoothDevice btDevice; serialDevice = btDevice.getDeviceForAddress(bluetoothAddy, bluetoothAdapter); } device = new SerialPort(serialDevice); if(!device->open()) { throw std::runtime_error("unable to open serial device " + serialDevice); } GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor()); g_io_add_watch(chan, G_IO_IN,(GIOFunc)gioPollingFunc, this); g_io_add_watch(chan,G_IO_HUP,(GIOFunc)gioPollingFunc,this); g_io_add_watch(chan,G_IO_ERR,(GIOFunc)gioPollingFunc,this); g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch. }