rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-958L: Some fixes for 15s hang and unplug/plug issue reported lately.

Browse Source 
This is not cleanedup but I want to get this in so other people can test.
This commit is contained in:
Mathieu Rondonneau 2013-06-01 22:13:04 -07:00
parent d0b0cb77d4
commit 5732c2fd4c
3 changed files with 153 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ground/openpilotgcs/src/plugins/ophid/inc/ophid_const.h
View File

@ -60,5 +60,6 @@
#define OPHID_ERROR_PARAMETER -3
#define OPHID_ERROR_HANDLE -4
#define OPHID_ERROR_INIT -5
#define OPHID_ERROR_ENUMERATION -6
#endif // OPHID_CONST_H

2
ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h
View File

@ -194,7 +194,7 @@ private:
*/
static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property);
static int infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index);
static void enumerateDevicesWin(const GUID & guidDev, QList<USBPortInfo> *infoList);
void enumerateDevicesWin(const GUID & guidDev);
bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam);
#ifdef QT_GUI_LIB
USBRegistrationWidget *notificationWidget;

225
ground/openpilotgcs/src/plugins/ophid/src/ophid_usbmon_win.cpp
View File

@ -36,6 +36,8 @@
#define printf qDebug
static QMutex DeviceChangeMutex;
void USBMonitor::deviceEventReceived()
{
qDebug() << "Device event";
@ -80,34 +82,34 @@ USBMonitor::~USBMonitor()
*/
QList<USBPortInfo> USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB)
{
QList<USBPortInfo> allPorts = availableDevices();
qDebug() << "USBMonitor::availableDevices list off all ports:";
qDebug() << "USBMonitor::availableDevices total ports:" << allPorts.length();
foreach(USBPortInfo info, allPorts) {
qDebug() << "----------";
qDebug() << "bcdDevice:" << info.bcdDevice;
qDebug() << "devicePath:" << info.devicePath;
qDebug() << "product:" << info.product;
}
qDebug() << "END OF LIST";
int i;
QList<USBPortInfo> thePortsWeWant;
qDebug() << "USBMonitor::availableDevices bcdLSB=" << bcdDeviceLSB;
foreach(USBPortInfo port, allPorts) {
qDebug() << "USBMonitorWin:Port VID=" << port.vendorID << "PID=" << port.productID << "bcddevice=" << port.bcdDevice;
if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) &&
((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) {
thePortsWeWant.append(port);
OPHID_TRACE("IN");
// Trigger enumeration
//OPHID_DEBUG("Enumerate");
//availableDevices();
// Print the list
qDebug() << "List off (" << knowndevices.length() << ") devices that are tracked:";
foreach(USBPortInfo info, knowndevices /*thePortsWeWant*/) {
qDebug() << "product:" << info.product
<< " bcdDevice:" << info.bcdDevice
<< " devicePath:" << info.devicePath;
//
if ((info.vendorID == vid || vid == -1) &&
(info.productID == pid || pid == -1) &&
((info.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) &&
((info.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) {
thePortsWeWant.append(info);
OPHID_DEBUG("Found device.");
}
}
qDebug() << "USBMonitor::availableDevices list off matching ports vid pid bcdMSD bcdLSD:" << vid << pid << bcdDeviceMSB << bcdDeviceLSB;
qDebug() << "USBMonitor::availableDevices total matching ports:" << thePortsWeWant.length();
foreach(USBPortInfo info, thePortsWeWant) {
qDebug() << "----------";
qDebug() << "bcdDevice:" << info.bcdDevice;
qDebug() << "devicePath:" << info.devicePath;
qDebug() << "product:" << info.product;
}
qDebug() << "END OF LIST";
OPHID_TRACE("OUT");
return thePortsWeWant;
}
@ -131,8 +133,8 @@ QList<USBPortInfo> USBMonitor::availableDevices(int vid, int pid, int bcdDeviceM
#define TCHARToQString(x) QString::fromLocal8Bit((x))
#define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y))
#endif /*UNICODE*/
#define MATCHOPHIDSTRING "COL01"
#define MATCHOPBLSTRING "161A0549"
#define HID2IGNOREUC "COL02"
#define HID2IGNORElc "col02"
void USBMonitor::setUpNotifications()
{
@ -160,6 +162,7 @@ void USBMonitor::setUpNotifications()
}
LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam)
{
OPHID_TRACE("IN");
if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) {
PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam;
if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) {
@ -169,34 +172,46 @@ LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam)
matchAndDispatchChangedDevice(deviceID, guid_hid, wParam);
}
}
OPHID_TRACE("OUT");
return 0;
}
#ifdef QT_GUI_LIB
bool USBRegistrationWidget::winEvent(MSG *message, long *result)
{
bool ret = false;
if (message->message == WM_DEVICECHANGE) {
OPHID_TRACE("IN");
qese->onDeviceChangeWin(message->wParam, message->lParam);
*result = 1;
return true;
ret = true;
OPHID_TRACE("OUT");
}
return false;
return ret;
}
#endif
bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam)
{
qDebug() << "USB_MONITOR matchAndDispatchChangedDevice deviceID=" << deviceID;
bool rv = false;
DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : DIGCF_ALLCLASSES;
bool rc;
DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : 0/*DIGCF_ALLCLASSES*/;
HDEVINFO devInfo;
// QString qHIDSymbolicName="HID";
// WCHAR wHIDSymbolicName[10];
// qHIDSymbolicName.toWCharArray(wHIDSymbolicName);
if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, dwFlag | DIGCF_DEVICEINTERFACE)) != INVALID_HANDLE_VALUE) {
SP_DEVINFO_DATA spDevInfoData;
spDevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
for (DWORD i = 0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) {
DWORD nSize = 0;
TCHAR buf[MAX_PATH];
if (SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) &&
deviceID.contains(TCHARToQString(buf)) && (deviceID.contains(MATCHOPHIDSTRING) ||
deviceID.contains(MATCHOPBLSTRING))) { // we found a match
rc = SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize);
qDebug() << "PATH:" << TCHARToQString(buf);
if (rc && deviceID.contains(TCHARToQString(buf)) /*&& !QString().contains(TCHARToQString(HID2IGNOREUC))*/) { // we found a match
qDebug() << "ACCEPTED" << TCHARToQString(buf);
USBPortInfo info;
info.devicePath = deviceID;
if (wParam == DBT_DEVICEARRIVAL) {
@ -205,46 +220,61 @@ bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const G
qDebug() << "USB_MONITOR infoFromHandle failed on matchAndDispatchChangedDevice";
break;
}
bool m_break = false;
foreach(USBPortInfo m_info, knowndevices) {
if (m_info.serialNumber == info.serialNumber && m_info.productID == info.productID && m_info.bcdDevice == info.bcdDevice && m_info.devicePath == info.devicePath) {
qDebug() << "USB_MONITOR device already present don't emit signal";
m_break = true;
if (knowndevices.length()) {
foreach(USBPortInfo m_info, knowndevices) {
qDebug() << m_info.devicePath;
qDebug() << info.devicePath;
if (m_info.serialNumber == info.serialNumber &&
m_info.productID == info.productID &&
m_info.bcdDevice == info.bcdDevice &&
m_info.devicePath == info.devicePath) {
qDebug() << "USB_MONITOR device already present don't emit signal";
break;
}
}
}
if (m_break) {
break;
}
}
if (info.bcdDevice == 0 || info.product.isEmpty()) {
qDebug() << "USB_MONITOR empty information on device not emiting signal";
break;
}
knowndevices.append(info);
qDebug() << "USB_MONITOR emit device discovered on device:" << info.product << info.bcdDevice;
qDebug() << "USB_MONITOR emit device discovered on device:"
<< info.product
<< info.bcdDevice;
emit deviceDiscovered(info);
break;
} else if (wParam == DBT_DEVICEREMOVECOMPLETE) {
bool found = false;
// DeviceChangeMutex.lock();
for (int x = 0; x < knowndevices.count(); ++x) {
if (knowndevices[x].devicePath == deviceID) {
// if (knowndevices[x].devicePath == deviceID) {
USBPortInfo temp = knowndevices.at(x);
knowndevices.removeAt(x);
qDebug() << "USB_MONITOR emit device removed on device:" << temp.product << temp.bcdDevice;
emit deviceRemoved(temp);
//emit deviceRemoved(temp);
found = true;
break;
}
// break;
// }
}
if (!found) {
qDebug() << "USB_MONITOR emit device removed on unknown device";
//if (!found) {
// qDebug() << "USB_MONITOR emit device removed on unknown device";
emit deviceRemoved(info);
}
//}
//else
break;
// DeviceChangeMutex.unlock();
}
break;
}
}
SetupDiDestroyDeviceInfoList(devInfo);
}
OPHID_TRACE("OUT");
return rv;
}
@ -259,44 +289,57 @@ QString USBMonitor::getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData
delete[] buff;
return result;
}
/**
Returns a list of all currently available devices
*/
QList<USBPortInfo> USBMonitor::availableDevices()
{
QList<USBPortInfo> ports;
enumerateDevicesWin(guid_hid, &ports);
// qDebug()<<"USBMonitorWin availabledevices="<<ports.count();
return ports;
QList<USBPortInfo> USBMonitor::availableDevices() {
enumerateDevicesWin(guid_hid);
return knowndevices;
}
void USBMonitor::enumerateDevicesWin(const GUID & guid, QList<USBPortInfo> *infoList)
void USBMonitor::enumerateDevicesWin(const GUID & guid)
{
HDEVINFO devInfo;
USBPortInfo info;
DWORD j = 0;
DWORD i = 0;
OPHID_TRACE("IN");
DeviceChangeMutex.lock();
// QString qHIDSymbolicName = "HID";
// WCHAR wHIDSymbolicName[10];
// qHIDSymbolicName.toWCharArray(wHIDSymbolicName);
// qDebug()<<"enumerateDevicesWin1";
if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE)) != INVALID_HANDLE_VALUE) {
// qDebug()<<"enumerateDevicesWin2";
SP_DEVINFO_DATA devInfoData;
devInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
for (DWORD i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) {
// only handle the first one enumerated that matches openpilot vendor
// the second one is expected to be the gamepad controller which we don't want to deal with
for (i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) {
int r = infoFromHandle(guid, info, devInfo, i);
if (r == 1) {
infoList->append(info);
} else if (r == 0) {
//infoList->append(info);
knowndevices.append(info);
j++;
// break;
} else if (r == OPHID_ERROR_ENUMERATION) {
break;
}
}
SetupDiDestroyDeviceInfoList(devInfo);
}
OPHID_DEBUG("Added %d device(s).", j);
DeviceChangeMutex.unlock();
OPHID_TRACE("OUT");
}
int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index)
{
OPHID_TRACE("IN");
// qDebug()<<"index0="<<index;
bool ret;
int ret;
HANDLE h;
SP_DEVICE_INTERFACE_DATA iface;
SP_DEVICE_INTERFACE_DETAIL_DATA *details;
@ -304,18 +347,21 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO &
HIDD_ATTRIBUTES attrib;
PHIDP_PREPARSED_DATA hid_data;
HIDP_CAPS capabilities;
QString qDevicePath;
iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
ret = SetupDiEnumDeviceInterfaces(devInfo, NULL, &guid, index, &iface);
if (!ret) {
return 0;
ret = OPHID_ERROR_ENUMERATION;
goto leave;
}
// qDebug()<<"index1="<<index;
SetupDiGetInterfaceDeviceDetail(devInfo, &iface, NULL, 0, &reqd_size, NULL);
details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size);
if (details == NULL) {
return 2;
ret = 2;
goto leave;
}
// qDebug()<<"index2="<<index;
memset(details, 0, reqd_size);
@ -323,10 +369,27 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO &
ret = SetupDiGetDeviceInterfaceDetail(devInfo, &iface, details, reqd_size, NULL, NULL);
if (!ret) {
free(details);
return 2;
ret = 2;
goto leave;
}
// qDebug()<<"index3="<<index;
h = CreateFile(details->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL);
qDevicePath = QString().fromWCharArray(details->DevicePath).toUpper();
// Exclude any non-openpilot devices
if (!qDevicePath.contains("VID_20A0")) {
ret = 2;
goto leave;
}
if (qDevicePath.contains("COL02")) {
ret = 2;
goto leave;
}
//.replace("#","\\").remove(0,4);
//WCHAR wDevicePath[256];
//qDevicePath.toWCharArray(wDevicePath);
qDebug() << "Found device with valid PATH: " << qDevicePath;
h = CreateFile(details->DevicePath /*DevicePath*/, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL);
if (h == INVALID_HANDLE_VALUE) {
DWORD err = GetLastError();
@ -335,13 +398,15 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO &
// Let's not log it :)
if (err == ERROR_ACCESS_DENIED) {
free(details);
return 2;
ret = 2;
goto leave;
}
qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath);
free(details);
return 2;
ret = 2;
goto leave;
}
// qDebug()<<"index4="<<index;
free(details);
@ -351,17 +416,25 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO &
info.vendorID = attrib.VendorID;
info.productID = attrib.ProductID;
info.bcdDevice = attrib.VersionNumber;
info.devicePath = qDevicePath;
if (attrib.VendorID != 0x20A0) {
CloseHandle(h);
ret = 2;
goto leave;
}
if (!ret || !HidD_GetPreparsedData(h, &hid_data)) {
CloseHandle(h);
return 2;
ret = 2;
goto leave;
}
// qDebug()<<"index5="<<index;
if (!HidP_GetCaps(hid_data, &capabilities)) {
HidD_FreePreparsedData(hid_data);
CloseHandle(h);
return 2;
ret = 2;
goto leave;
}
// qDebug()<<"index6="<<index;
info.UsagePage = capabilities.UsagePage;
@ -377,5 +450,9 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO &
// qDebug()<<"index="<<index<<"ProductID="<<info.product;
CloseHandle(h);
h = NULL;
return 1;
ret = 1;
leave:
OPHID_TRACE("OUT");
return ret;
}