Introduction
In this article, I will attempt to give a beginner level example for a Windows sensor device driver. I cannot even start to explain how and why I did each and every thing but I will try to give a reference to the place I found the information. The driver in this example is for operating the iNemo evaluation board by ST. The sensor driver in this example is a user mode driver (UMD) stacked over WinUSB as the function driver, and it exposes its functionality according to the Microsoft Sensor and Location API.
Background
Windows Sensor and Location API is relatively new, and as far as I can say from my experience - not as mature as other Microsoft APIs. This is actually the main reason that drove me into writing this article. When I started coding it, the only two examples I could find were the WiiMote driver here on CodeProject and the built in example in the Windows driver kit (WinDDK/WDK). I personally prefer the WinDDK coding style rather than the WiiMote style so you will find my code very similar to the WinDDK code.
The iNemo is a nice evaluation board by ST which I found to be quite friendly if you want to play with sensors a little. It connects to the host machine over USB and its firmware utilizes it as if it were a virtual COM port (VCP). This makes it quite easy to deal with from the Driver's point of view since it is as simple as IO to a file.
Talking about a sensor driver requires also a word or two about the sensors themselves. There is a lot to say about each sensor and each of the different methods to use its data, but the main thing to remember about sensors is that they are sensors - a physical device that is prone to error and can behave differently according to the situation at the same moment. I am saying that because many programmers are not aware of it and treat sensors as a deterministic device just like another system device. This approach often leads to a very poor user experience. To be more specific, I will give some examples:
- Barometer (pressure sensor) - Treated as an altimeter, but is effected by the weather at the same moment and worse - effected by wind blows.
- Accelerometer (G sensor)- Treated as G sensor but actually measures the specific force on the device. It is very sensitive to vibrations.
- Gyroscope - Usually measures the angular rates. Even the smallest bias of the device (and there is always such bias) will lead to drift unless treated algorithmically.
- Magnetometer (compass)- Gives a reading of the magnetic flux/field, but as any standard magnetic compass, it is sensitive to electrical currents and magnets nearby.
Using the Code
Driver Architecture
There are different ways to approach Windows driver programming. You can use KMDF, UMDF, both, and organize your driver stack in various ways. As most driver developers, I think that whatever you can do in the user space should not be in the kernel space. This is the main reason Microsoft had developed the UMDF. It reduces system crashes and it is easier to develop and debug. In this example, we deal with a very simple device which communicates with the host machine over USB, no network, graphics, interrupts, etc. This is a classic case of a driver that should be completely UMD. What I show in this example is a UMD, stacked over WinUSB. In case you are new to UMDF, I found the following document very informative and easy to get started with: UMDF-arch. If you are an absolute rookie in driver programming, you can get started either with DrvDev_Intro doc by Microsoft or Toby Opferman's article, both are very informative.
What's in the Source Files?
You will find that the file structure is very similar to what you will find in the WDK source directory, I used it a lot as a reference here. The functionality is divided in the following way:
- Driver.cpp - Initializations which are common to I think all or at least most of the UMDs you will see. You can find other code styles for the same initializations in the WDK source examples. In here, I chose the one I found to be more convenient and modern which is with the COM macros.
- Device.cpp - All the functionality that is related to operating the USB device itself is here.
- SensorDdi.cpp - As its name suggests, it exposes the sensor API.
- All other files are either common or used for the build. Such files are used all across the WDK example source code so you find your way there.
- MySensor.inx - The source for the build-generated .inf file. I mention this file in particular because without it, you cannot install the driver and writing .inx/.inf file is not trivial at first for a beginner.
What Does this Code Do?
The code in this article is used as a UMD sensor device driver for the iNemo evaluation board. it initializes the device and gets sensor data from it asynchronously at a constant 50Hz rate until the device is removed/uninstalled. It is by no means a "production-ready" code so don't bother complaining that it does not check every little thing or manage power consumption because it does not do that. It is the most basic driver that will make the device identify itself as a sensor and output the data at 50Hz according to the sensor API.
Building the Code
To build the code, you would need the WDK installed. To build it, open the relevant build environment according to the target system and "build -ceZ" in the source directory.
Installing the Driver
For installing the driver, you would need 5 files in total:
- MySensor.inf - from the build output
- MySensor.dll - from the build output
- WdfCoInstaller01009.dll - from the WDK /redist/wdf directory
- WUDFUpdate_01009.dll - from the WDK /redist/wdf directory
- winusbcoinstaller2.dll - from the WDK /redist/winusb directory
Locate all the files in the same directory and point the windows driver update wizard to the .inf file like any other driver.
The WinUSB
WinUSB is a generic USB device (kernel) driver which gives you several options to use it, depending on your needs. You can use it either as a function driver (like I did in this example), as a part of it, from UMDF or KMDF, or you could use it as the whole driver and write only the .inf file for it. To understand how to integrate it into your .inf file, you can just take a look at the .inx file in the attached source or in the generated .inf after you build, it is quite straight forward. Before I start going over the code, you can find a more detailed reference on WinUSB usage, MSDN of course has it all. In particular, I would recommend the following document: WinUSB HowTo. For function documentation, refer to the WDK help file - it is good enough in that manner.
Initializing the WinUSB
PWSTR deviceName = NULL;
DWORD deviceNameCch = 0;
hr = m_pWdfDevice->RetrieveDeviceName(NULL, &deviceNameCch);
deviceName = new WCHAR[deviceNameCch];
if (deviceName == NULL) {
hr = E_OUTOFMEMORY;
}
if (SUCCEEDED(hr))
{
hr = m_pWdfDevice->RetrieveDeviceName(deviceName, &deviceNameCch);
}
if (SUCCEEDED(hr))
{
m_Handle = CreateFile( deviceName,
GENERIC_WRITE | GENERIC_READ,
FILE_SHARE_WRITE | FILE_SHARE_READ,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED,
NULL);
}
BOOL CMyDevice::Initialize_Device()
{
m_bResult = FALSE;
USB_INTERFACE_DESCRIPTOR ifaceDescriptor = {0,0,0,0,0,0,0,0,0};
WINUSB_PIPE_INFORMATION pipeInfo;
UCHAR speed = 0;
ULONG length = 0;
m_bResult = WinUsb_Initialize(m_Handle, &m_UsbHandle[0]);
if(m_bResult)
{
m_bResult = WinUsb_GetAssociatedInterface(m_UsbHandle[0],0,&m_UsbHandle[1]);
}
for( int k =0; k<2;k++)
{
if(m_bResult)
{
length = sizeof(UCHAR);
m_bResult = WinUsb_QueryDeviceInformation(m_UsbHandle[k],
DEVICE_SPEED,
&length,
&speed);
}
if(m_bResult)
{
m_Speed = speed;
m_bResult = WinUsb_QueryInterfaceSettings(m_UsbHandle[k],
0,
&ifaceDescriptor);
}
if(m_bResult)
{
for(int i=0;i<ifaceDescriptor.bNumEndpoints;i++)
{
m_bResult = WinUsb_QueryPipe(m_UsbHandle[k],
0,
(UCHAR) i,
&pipeInfo);
if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId))
{
m_bulkInPipe = pipeInfo.PipeId;
m_bulkInPipePacketSize =
pipeInfo.MaximumPacketSize;
}
else if(pipeInfo.PipeType == UsbdPipeTypeBulk &&
USB_ENDPOINT_DIRECTION_OUT(pipeInfo.PipeId))
{
m_bulkOutPipe = pipeInfo.PipeId;
}
else if(pipeInfo.PipeType == UsbdPipeTypeInterrupt)
{
m_interruptPipe = pipeInfo.PipeId;
}
else
{
m_bResult = FALSE;
break;
}
}
}
}
return m_bResult;
}
The first code snippet here is actually just preparation for the init flow. All we do in here is actually create a file for I/O management with the WinUSB. The file handle is used later on by WinUsb_Initialize. If you use WinUSB from a KMD, you might create the file in another way.
The second code snippet is the actual initialization of the WinUSB and it is composed of a few simple actions that might vary according to the hardware you are using - depending on the number of interfaces/pipes and their types. The main actions we do are:
- Get a
PWINUSB_INTERFACE_HANDLE using WinUsb_Initialize. - Get another
PWINUSB_INTERFACE_HANDLE for the second interface of our device using WinUsb_GetAssociatedInterface. This might change in other devices of course. - Get handles for the different pipes and identify them as bulk-in bulk-out or interrupt using
WinUsb_QueryDeviceInformation and WinUsb_QueryInterfaceSettings. this part is tailor made for this device, other devices might have more interfaces/pipes.
Basic read/write
There are several places in the Device.cpp where I use basic read/writes to the device, we can take the following for instance:
BOOL CMyDevice::iNemoFrameWrite(UCHAR MsgId)
{
USHORT bufSize = 3;
UCHAR szBuffer[3];
ULONG bytesWritten = 0;
ULONG bytesRead = 0;
m_bResult = FALSE;
szBuffer[FRM_CTL] = 0x20; szBuffer[LENGTH] = 0x01;
szBuffer[MSG_ID] = MsgId;
m_bResult = WinUsb_WritePipe(m_UsbHandle[1],
m_bulkOutPipe,
szBuffer,
bufSize,
&bytesWritten,
NULL);
if(m_bResult && (bytesWritten == bufSize))
{
m_bResult = WinUsb_ReadPipe(m_UsbHandle[1],
m_bulkInPipe,
szBuffer,
bufSize,
&bytesRead,
NULL);
}
if(m_bResult)
{
if( (szBuffer[FRM_CTL] == FRM_CTL_ACK) &&
(szBuffer[LENGTH] == 0x01) && (szBuffer[MSG_ID] == MsgId))
{
m_bResult = TRUE;
}
else if( (szBuffer[FRM_CTL] == FRM_CTL_NACK) &&
(szBuffer[LENGTH] == 0x02) && (szBuffer[MSG_ID] == MsgId))
{
m_bResult = WinUsb_ReadPipe(m_UsbHandle[1],
m_bulkInPipe,
szBuffer,
1,
&bytesRead,
NULL);
if(m_bResult)
{
m_hr = szBuffer[0];
}
m_bResult = FALSE;
}
else
{
m_bResult = FALSE;
}
}
return m_bResult;
}
You can disregard all the ACK/NACK and other protocol stuff because they are not USB related (iNemo communication protocol between firmware and driver). The read/write actions are actually as easy as reading/writing to any other file, using the WinUsb_ReadPipe and WinUsb_WritePipe. It is quite straightforward so I don't see any point in getting any deeper.
Asynchronous read
The asynchronous read is actually not complicated because of the WinUSB, but rather because of threading and synchronization issues. I will not get into the synchronization issues because they are rather general and not particular to this use case. In here, I avoided races and collisions just by using either sync. read/writes during init/release or async. reads during up-time, never together. The code snippet below is run by a worker thread, and the main difference from sync read/writes is the use of the events. Everything you'll see before the while loop is just initializations but you should pay attention to the hevents and oOverlap which are used later. The interesting stuff happens in the while loop. You can see that it is an infinite loop of calls to WaitForMultipleObjects. What we wait for is either an event initiated from the main driver thread, telling us to stop this thread, or a read event coming from the WinUSB. What I do with the read event is related more to the iNemo communication protocol, you can do whatever fits you in here. The read event is fired according to the terms I defined which is reading 3 bytes which are the iNemo protocol header.
BOOL CMyDevice::ReadHeader(IN PUCHAR Buffer, IN ULONG bufSize,
OUT PULONG bytesRead, IN LPOVERLAPPED Overlapped)
{
return ReadBuffer(Buffer, bufSize, bytesRead, Overlapped);
}
BOOL CMyDevice::ReadBuffer(IN PUCHAR Buffer, IN ULONG bufSize,
OUT PULONG bytesRead, IN LPOVERLAPPED Overlapped)
{
return WinUsb_ReadPipe(m_UsbHandle[1],
m_bulkInPipe,
Buffer,
bufSize,
bytesRead,
Overlapped);
}
DWORD WINAPI CMyDevice::AsyncReadThreadProc(__in LPVOID pvData)
{
OVERLAPPED oOverlap;
HANDLE hEvents[2];
DWORD dwWait = 0;
DWORD dwNum = 0;
const DWORD STOP_THREAD = WAIT_OBJECT_0;
const DWORD READ_EVENT = WAIT_OBJECT_0+1;
USHORT headerSize = 3;
UCHAR buffer[3];
ULONG bytesRead = 0;
BOOL b = FALSE;
CMyDevice* pThis = static_cast<cmydevice* />(pvData);
HRESULT hr = CoInitializeEx(NULL, COINIT_MULTITHREADED);
pThis->m_hReadAsync = CreateEvent(
NULL, TRUE, FALSE, NULL);
hEvents[0] = pThis->m_hCloseThread;
hEvents[1] = pThis->m_hReadAsync;
oOverlap.hEvent = pThis->m_hReadAsync;
oOverlap.Internal = 0;
oOverlap.InternalHigh = 0;
oOverlap.Offset = 0;
oOverlap.OffsetHigh = 0;
if (SUCCEEDED(hr))
{
while (true)
{
pThis->m_bResult = ResetEvent(pThis->m_hReadAsync);
pThis->ReadHeader(buffer, headerSize, &bytesRead, &oOverlap);
dwWait = WaitForMultipleObjects( 2, hEvents, FALSE, INFINITE );
b = WinUsb_GetOverlappedResult(pThis->m_UsbHandle[1],
&oOverlap,&dwNum,FALSE);
switch(dwWait)
{
case STOP_THREAD:
CoUninitialize();
CloseHandle(pThis->m_hReadAsync);
return 0;
case READ_EVENT:
if(b)
{
pThis->ParseDataMessage(buffer);
}
else
{
WinUsb_FlushPipe (pThis->
m_UsbHandle[1],pThis->m_bulkInPipe);
}
break;
default:
break;
}
}
}
return 1;
};
Releasing WinUSB
The only thing left with the WinUSB is to release it when we're done, using WinUsb_Free. Notice that here too, there is a separation between the synchronous and asynchronous WinUSB activity. The device will keep sending me data until it gets the "stop" command but it doesn't matter because we don't care about this data anymore.
HRESULT
CMyDevice::OnD0Exit(
__in IWDFDevice* pWdfDevice,
__in WDF_POWER_DEVICE_STATE previousState
)
{
UNREFERENCED_PARAMETER(pWdfDevice);
UNREFERENCED_PARAMETER(previousState);
if(NULL != m_hCloseThread)
{
::SetEvent(m_hCloseThread);
::WaitForSingleObject(m_hEventThread, INFINITE);
if (NULL != m_hEventThread)
{
CloseHandle(m_hEventThread);
m_hEventThread = NULL;
}
if(m_iNemoStarted)
{
m_iNemoStarted = FALSE;
Stop_iNemo();
}
CloseHandle(m_hCloseThread);
m_hCloseThread = NULL;
if(m_UsbHandle[0])
{
WinUsb_Free(m_UsbHandle[0]);
}
if(m_UsbHandle[1])
{
WinUsb_Free(m_UsbHandle[1]);
}
}
return S_OK;
}
Power States and UMDF Callbacks
One of the annoying things in driver programming compared to a simple "hello world" code, is that you cant find your legs and hands in all of these callbacks. It is quite hard at the beginning to understand what function is called first and what comes after and in what order. Getting to know the callbacks is significant here because we are dealing with a USB device that can come and go, be removed without notice and implement power management techniques, so I will try to highlight the main callbacks. If you would take a look at the UMDF-arch document, you will find some more in-depth explanation. The following diagrams from this document summarize the whole flow nicely.
Device Arrival
IDriverEntry::OnDeviceAdd is where you create the instance of your device, creating an instance obviously calls the constructor of your device class so you can guess when that will run. The PnpCallbackHardware::OnPrepareHardware is where the main initializations of the device should be rather then the device class constructor. In this example, we also declare use of the IPnPCallback interface to support disable/enable of the device without plugging it out. So you can see that the initialization in our case is separated to both the OnPrepareHardware and the OnD0Entry. From this stage and on, the other callbacks are not required for the basic functionality we need.
Device Removal
In here you could see that it is quite straight forward, calling the callbacks in the opposite order. You probably already understand that however you partition your initializations between the device arrival callbacks, you need to keep the same partitioning here if you want it to work properly.
Device Surprise Removal
This is quite similar to the standard removal so no point in getting deeper.
The Sensor API
There is quite a lot to talk about the sensor API itself since it is a relatively new API and every vendor tries to push its own API in its OS, so you can debate about how Microsoft is doing something the right or the wrong way. You could see some of my complaints in the MSDN forum. I can tell you that we already contacted the Microsoft guys through the appropriate channels about these issues so they are aware of them and hopefully will see right to fix at least some of them in the next generation of the API (Win8?). Regardless of my comments about the API itself, let's see some code...
The registration of the device as one which corresponds to the API is done in the SensorDdi.h file in the following way:
BEGIN_COM_MAP(CSensorDdi)
COM_INTERFACE_ENTRY(ISensorDriver)
END_COM_MAP()
The implementation of the ISensorDriver requires the implementation of the following functions:
HRESULT STDMETHODCALLTYPE OnGetSupportedSensorObjects(
__out IPortableDeviceValuesCollection** ppSensorObjectCollection
);
HRESULT STDMETHODCALLTYPE OnGetSupportedProperties(
__in LPWSTR pwszObjectID,
__out IPortableDeviceKeyCollection** ppSupportedProperties
);
HRESULT STDMETHODCALLTYPE OnGetSupportedDataFields(
__in LPWSTR pwszObjectID,
__out IPortableDeviceKeyCollection** ppSupportedDataFields
);
HRESULT STDMETHODCALLTYPE OnGetSupportedEvents(
__in LPWSTR pwszObjectID,
__out GUID** ppSupportedEvents,
__out ULONG* pulEventCount
);
HRESULT STDMETHODCALLTYPE OnGetProperties(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID,
__in IPortableDeviceKeyCollection* pProperties,
__out IPortableDeviceValues** ppPropertyValues
);
HRESULT STDMETHODCALLTYPE OnGetDataFields(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID,
__in IPortableDeviceKeyCollection* pDataFields,
__out IPortableDeviceValues** ppDataValues
);
HRESULT STDMETHODCALLTYPE OnSetProperties(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID,
__in IPortableDeviceValues* pPropertiesToSet,
__out IPortableDeviceValues** ppResults
);
HRESULT STDMETHODCALLTYPE OnClientConnect(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID
);
HRESULT STDMETHODCALLTYPE OnClientDisconnect(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID
);
HRESULT STDMETHODCALLTYPE OnClientSubscribeToEvents(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID
);
HRESULT STDMETHODCALLTYPE OnClientUnsubscribeFromEvents(
__in IWDFFile* pClientFile,
__in LPWSTR pwszObjectID
);
HRESULT STDMETHODCALLTYPE OnProcessWpdMessage(
__in IUnknown* pUnkPortableDeviceValuesParams,
__in IUnknown* pUnkPortableDeviceValuesResults
);
The code itself is a little hard to read because of massive use of collections and stuff which are not straightforward for beginners so I will try to highlight the main interesting things here.
The OnGetSupportedSensorObjects is the place to actually tell the world how many sensor objects you have. To be clearer, I have a single USB device but in the code it declares having 3 different sensors or sensor objects. So you don't need to have separate entries in the device manager, it is enough to have one and let it deal with all the different sensor types you wish.
The OnGetSupportedProperties, OnGetProperties, OnGetSupportedDataFields and OnSetProperties are quite straightforward. They basically declare the device properties and data fields, and return "not supported" on a try to change them (you can support change of course, I don't do that in this example). OnProcessWpdMessage as you see is just returning S_OK. OnGetSupportedEvents is trivial as well, declaring the supported events.
OnClientSubscribeToEvents, OnClientUnsubscribeFromEvents, OnClientConnect and OnClientDisconnect are not very interesting in our example, but it might be interesting if you wish to implement some power management. If you don't have any clients, you don't need to keep the device working nor fire the events. That's a good place to start the cuttings from.
The last and maybe the most important is the OnGetDataFields, after all, that's why we have the sensor for. A very important point to notice here, is that you must have a timestamp associated with your sample. If you will go over the TimeSensor example from the WDK source and try to manipulate it to your own use, you might think "hey! I don't need that, I don't measure time...". Don't. Another point of interest is the return type of each data field. To date, there are contradictions between some of them if you compare the WDK and the MSDN and the Windows API code pack examples. Anyway, I stuck with the WDK documentation, as far as I know this documentation has the final say in the debate.
The returned values are pre-calculated on data arrival from the USB (in Device.cpp) for efficiency. The calculation itself maybe has the most influence on the user experience of your hardware. You can have the most advanced hardware but if your algorithm is crappy, so will the user experience be. Personally, I think that the API should expose the most basic measurements without any manipulation to the applications and optionally expose extra algorithmic data so the application programmer could choose whatever fits his needs. However, it seems like the Microsoft guys don't think like I do so for the compass data you have to fuse the data from the magnetometers and the accelerometers. In the code, you can see the basic way to do this kind of data fusion (plus some safety for division by 0), located in CMyDevice::ParseDataMessage. by the way, what I am saying now might be completely irrelevant for you if all you need is temperature reads for example. I am assuming that most of the readers are more interested in orientation sensing. The technique showed here should be enough for basic application, you could fly an AR.Drone with this data quality without a problem. However, you might want to read some more about more advanced fusion techniques. I would recommend Extended Kalman Filter (EKF) as a good starter. Notice that getting an EKF down to code is not so complicated, but for generating the equations for it and calibrating the parameters you should know how to use the theory behind it. Writing a worthy EKF is an art for itself.
I think that some readers will be interested in this code but will not bother to download the source so you can see it below. notice that I reluctantly had to do some ugly things like approximate derivatives and such, just to fit the API. I really think the Microsoft guys should change that. Note that I use the aeronautic NASA convention with my data which is the most standard way you'll find, so when you see that I insert some '-' signs here and there, it's to straighten up the data in the direction I want it to be. You would probably need to redefine the signs if you use another hardware under the hood. The code:
void CMyDevice::ParseDataMessage(IN PUCHAR HeaderBuffer)
{
USHORT bufSize = 20; UCHAR buffer[20]; ULONG bytesRead = 0;
BOOL bres = FALSE;
BOOL MagRawData = FALSE;
DOUBLE accX = 0;
DOUBLE accY = 0;
DOUBLE accZ = 0;
DOUBLE gyrX = 0;
DOUBLE gyrY = 0;
DOUBLE gyrZ = 0;
FLOAT magX = 0;
FLOAT magY = 0;
FLOAT magZ = 0;
if((HeaderBuffer[0] == 0x40) && (HeaderBuffer[1] == 0x15) &&
(HeaderBuffer[2] == 0x52))
{
bres = ReadBuffer(buffer, bufSize, &bytesRead, NULL);
if(bres)
{
SwapBytes(buffer, bufSize);
PSHORT raw_data = (PSHORT)buffer;
accX = -(DOUBLE)raw_data[1] * ACC_SCALE;
accY = (DOUBLE)raw_data[2] * ACC_SCALE;
accZ = (DOUBLE)raw_data[3] * ACC_SCALE;
gyrX = (-(DOUBLE)raw_data[4] - m_gyrXold) /
DeltaT; gyrY = (-(DOUBLE)raw_data[5] - m_gyrYold) /
DeltaT; gyrZ = (-(DOUBLE)raw_data[6] - m_gyrZold) /
DeltaT; m_gyrXold = -(DOUBLE)raw_data[4];
m_gyrYold = -(DOUBLE)raw_data[5];
m_gyrZold = -(DOUBLE)raw_data[6];
magX = (FLOAT)raw_data[7];
magY = -(FLOAT)raw_data[8];
magZ = -(FLOAT)raw_data[9];
if(!MagRawData)
{
DOUBLE teta = 0;
DOUBLE phy = 0;
DOUBLE psi = 0;
DOUBLE acc_norm = sqrt
(accX * accX + accY * accY + accZ * accZ);
DOUBLE accXnormed = accX / acc_norm;
DOUBLE accYnormed = accY / acc_norm;
DOUBLE mag_norm = sqrt
(magX * magX + magY * magY + magZ * magZ);
DOUBLE magXnormed = magX / mag_norm;
DOUBLE magYnormed = magY / mag_norm;
DOUBLE magZnormed = magZ / mag_norm;
DOUBLE Xh = 0;
DOUBLE Yh = 0;
DOUBLE rad2deg = 180/PI;
teta = asin(-accXnormed);
if (teta < PI/2 - 0.001 || teta > -PI/2 + 0.001)
{
phy = asin(accYnormed / cos(teta));
m_phyold = phy;
}
else
{
phy = m_phyold;
}
Xh = magXnormed * cos(teta) + magZnormed * sin(teta);
Yh = magXnormed * sin(phy) * sin(teta) +
magYnormed * cos(phy) - magZnormed *
sin(phy) * cos(teta);
if (Xh < 0.001 && Xh > -0.001)
{
if (Yh > 0)
{
psi = PI / 2;
}
else
{
psi = 3 * PI / 2;
}
}
else if (Yh == 0)
{
if (Xh > 0)
{
psi = 0;
}
else
{
psi = PI;
}
}
else
{
if (Xh > 0 && Yh > 0)
{
psi = 2*PI-atan(Yh / Xh);
}
else if (Xh > 0 && Yh < 0)
{
psi = atan(-Yh / Xh);
}
else if (Xh < 0 && Yh > 0)
{
psi = PI + atan(Yh / (-Xh));
}
else {
psi = PI - atan(-Yh / (-Xh));
}
}
magX = (FLOAT)(phy*rad2deg);
magY = (FLOAT)(teta*rad2deg);
magZ = (FLOAT)(psi*rad2deg);
}
m_pSensorDdi->SetDataTimeStamp();
m_pSensorDdi->SetAccData(accX,accY,accZ);
m_pSensorDdi->SetGyrData(gyrX,gyrY,gyrZ);
m_pSensorDdi->SetMagData(magX,magY,magZ);
m_pSensorDdi->PostDataEvent(0);
m_pSensorDdi->PostDataEvent(1);
m_pSensorDdi->PostDataEvent(2);
}
else
{
WinUsb_FlushPipe (m_UsbHandle[1],m_bulkInPipe);
}
}
else
{
WinUsb_FlushPipe (m_UsbHandle[1],m_bulkInPipe);
}
return;
}
iNemo Protocol
I don't want to get deep into this because the iNemo communication protocol is well defined and easy to understand from the ST documentation, I will only go over the flow I use in general. Well, it is quite straight forward, right after the init of WinUSB, I send the sequence: iNEMO_Connect, iNEMO_Set_Output_Mode, iNEMO_Start_Acquisition. Playing with the command parameters is quite straightforward as well, I just set the firmware to send me the data of the accelerometer, gyroscope and magnetometer at a 50Hz rate and tell it to start sending. At this point, I will move to reading the data asynchronously up until the device is disabled/removed. There, I will only send the iNEMO_Stop_Acquisition if it is still connected (synchronously) and voila!
Conclusion
In this article, I tried to give you some good references on things like WinUSB, Sensor API, UMDF. Hope I managed to do it successfully and that it was of use for you. You're welcome to contact me or just leave replies for feedback or whatever so I could add/change what's needed. That's all folks!
History
- 1st November, 2011: Initial version
