diff options
| author | Michelle Bergeron <[email protected]> | 2017-08-25 14:58:26 -0700 |
|---|---|---|
| committer | Michelle Bergeron <[email protected]> | 2017-08-25 14:58:26 -0700 |
| commit | 037d71513ccc6149d3ec6633c7c5574df7903e86 (patch) | |
| tree | 792619fc57013cfc06f0b9b3bd2387c3417afbf7 | |
| parent | 806725d560858c2018611fc4e7693d56bb2d29d5 (diff) | |
Add Fusion Sensor sample
| -rw-r--r-- | sensors/Fusion/Device.h | 199 | ||||
| -rw-r--r-- | sensors/Fusion/Driver.h | 19 | ||||
| -rw-r--r-- | sensors/Fusion/FusionSensor.ctl | 1 | ||||
| -rw-r--r-- | sensors/Fusion/FusionSensor.def | 6 | ||||
| -rw-r--r-- | sensors/Fusion/FusionSensor.inx | bin | 0 -> 4990 bytes | |||
| -rw-r--r-- | sensors/Fusion/FusionSensor.sln | 28 | ||||
| -rw-r--r-- | sensors/Fusion/FusionSensor.vcxproj | 203 | ||||
| -rw-r--r-- | sensors/Fusion/FusionSensor.vcxproj.Filters | 43 | ||||
| -rw-r--r-- | sensors/Fusion/HardwareSimulator.h | 60 | ||||
| -rw-r--r-- | sensors/Fusion/SensorsTrace.h | 102 | ||||
| -rw-r--r-- | sensors/Fusion/client.cpp | 789 | ||||
| -rw-r--r-- | sensors/Fusion/device.cpp | 729 | ||||
| -rw-r--r-- | sensors/Fusion/driver.cpp | 76 | ||||
| -rw-r--r-- | sensors/Fusion/hardwaresimulator.cpp | 277 | ||||
| -rw-r--r-- | sensors/Fusion/readme.md | 3 |
15 files changed, 2535 insertions, 0 deletions
diff --git a/sensors/Fusion/Device.h b/sensors/Fusion/Device.h new file mode 100644 index 00000000..1bd39606 --- /dev/null +++ b/sensors/Fusion/Device.h @@ -0,0 +1,199 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved +// +//Abstract: +// +// This module contains the type definitions for the client +// driver's device callback class. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#pragma once + +#include <windows.h> +#include <wdf.h> +#include <math.h> + +#include "SensorsTrace.h" +#include <SensorsCx.h> +#include <SensorsUtils.h> +#include <Math3DHelper.h> +#include <SensorsCx.h> +#include <SensorsDef.h> +#include <SensorsTrace.h> +#include <SensorsUtils.h> +#include <Math3DHelper.h> + +#define SENSOR_POOL_TAG_FUSIONSENSOR 'esuF' + +#define FusionSensor_Default_MinDataInterval_Ms (16) // 60Hz +#define FusionSensor_Default_DataInterval (100) // 10Hz +#define FusionSensor_Quarternion_Maximum (1.0f) +#define FusionSensor_Quarternion_Minimum (-1.0f) +#define FusionSensor_Quarternion_Resolution ((FusionSensor_Quarternion_Maximum-FusionSensor_Quarternion_Minimum)/65536) + + +// TODO: Customize the following 5 lines based on the hardware being used +#define GyrFakeDevice_Minimum_DegreesPerSecond (-2000.0f) +#define GyrFakeDevice_Maximum_DegreesPerSecond (2000.0f) +#define GyrFakeDevice_Precision (65536.0f) // 65536 = 2^16, 16 bit data +#define GyrFakeDevice_Range_DegreesPerSecond (GyrFakeDevice_Maximum_DegreesPerSecond - GyrFakeDevice_Minimum_DegreesPerSecond) +#define GyrFakeDevice_Resolution_DegreesPerSecond (GyrFakeDevice_Range_DegreesPerSecond / GyrFakeDevice_Precision) + +// TODO: Customize the following 3 lines based on the hardware being used +#define AccFakeDevice_Axis_Resolution (4.0f / 65536.0f) // in delta g +#define AccFakeDevice_Axis_Minimum (-2.0f) // in g +#define AccFakeDevice_Axis_Maximum (2.0f) // in g + +// Sensor Common Properties +typedef enum +{ + SENSOR_PROPERTY_STATE = 0, + SENSOR_PROPERTY_MIN_INTERVAL, + SENSOR_PROPERTY_MAX_DATAFIELDSIZE, + SENSOR_PROPERTY_SENSOR_TYPE, + SENSOR_PROPERTY_USE_GYRO, + SENSOR_PROPERTIES_COUNT +} SENSOR_COMMON_PROPERTIES_INDEX; + +// Sensor Enumeration Properties +typedef enum +{ + SENSOR_TYPE_GUID = 0, + SENSOR_MANUFACTURER, + SENSOR_MODEL, + SENSOR_PERSISTENT_UNIQUEID, + SENSOR_ISPRIMARY, + SENSOR_ENUMERATION_PROPERTIES_COUNT +} SENSOR_ENUMERATION_PROPERTIES_INDEX; + +// Accelerometer related data-field Properties +typedef enum +{ + SENSOR_ACC_RESOLUTION = 0, + SENSOR_ACC_MIN_RANGE, + SENSOR_ACC_MAX_RANGE, + SENSOR_ACC_DATA_FIELD_PROPERTY_COUNT +} SENSOR_ACC_DATA_FIELD_PROPERTY_INDEX; + +// Gyroscope related data-field Properties +typedef enum +{ + SENSOR_GYR_RESOLUTION = 0, + SENSOR_GYR_MIN_RANGE, + SENSOR_GYR_MAX_RANGE, + SENSOR_GYR_DATA_FIELD_PROPERTY_COUNT +} SENSOR_GYR_DATA_FIELD_PROPERTY_INDEX; + +// Supported Data +typedef enum +{ + FUSIONSENSOR_DATA_TIMESTAMP = 0, + FUSIONSENSOR_DATA_QUATERNION_W, + FUSIONSENSOR_DATA_QUATERNION_X, + FUSIONSENSOR_DATA_QUATERNION_Y, + FUSIONSENSOR_DATA_QUATERNION_Z, + FUSIONSENSOR_DATA_ACCURACY, + FUSIONSENSOR_DATA_DECLINATION_ANGLE, + FUSIONSENSOR_DATA_COUNT +} FUSIONSENSOR_DATA_INDEX; + +typedef enum +{ + FUSIONSENSOR_THRESHOLD_ROTATION_ANGLE = 0, + FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_X, + FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Y, + FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Z, + FUSIONSENSOR_THRESHOLD_ROTATION_RATE_X, + FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Y, + FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Z, + FUSIONSENSOR_THRESHOLD_COUNT +} FUSIONSENSOR_THRESHOLD_INDEX; + +typedef struct FusionSensorSample +{ + FILETIME Timestamp; + QUATERNION Quaternion; + ULONG Accuracy; + FLOAT DeclinationAngle; +} FusionSensorSample, *PFusionSensorSample; + +// +// Fusion Threshold +// +typedef struct _FusThreshold +{ + FLOAT RotationAngle; + VEC3D LinearAcceleration; + VEC3D RotationRate; +} FusThreshold, *PFusThreshold; + +typedef class FusionSensorDevice +{ +private: + // Simulator + WDFOBJECT m_SimulatorInstance; + + // WDF + WDFDEVICE m_FxDevice; + SENSOROBJECT m_SensorInstance; + WDFWAITLOCK m_Lock; + WDFTIMER m_Timer; + + // Sensor operation + BOOLEAN m_PoweredOn; + BOOLEAN m_Started; + ULONG m_Interval; + + BOOLEAN m_FirstSample; + ULONG m_StartTime; + ULONGLONG m_SampleCount; + + FusThreshold m_CachedThresholds; + + FusionSensorSample m_LastSample; + + // Sensor Specific Properties + PSENSOR_COLLECTION_LIST m_pEnumerationProperties; + PSENSOR_COLLECTION_LIST m_pProperties; + PSENSOR_PROPERTY_LIST m_pSupportedDataFields; + PSENSOR_COLLECTION_LIST m_pAccDataFieldProperties; // Accelerometer related datafield properties + PSENSOR_COLLECTION_LIST m_pGyrDataFieldProperties; // Gyroscope related datafield properties + PSENSOR_COLLECTION_LIST m_pThresholds; + PSENSOR_COLLECTION_LIST m_pData; + +public: + // WDF callbacks + static EVT_WDF_DRIVER_DEVICE_ADD OnDeviceAdd; + static EVT_WDF_DEVICE_PREPARE_HARDWARE OnPrepareHardware; + static EVT_WDF_DEVICE_RELEASE_HARDWARE OnReleaseHardware; + static EVT_WDF_DEVICE_D0_ENTRY OnD0Entry; + static EVT_WDF_DEVICE_D0_EXIT OnD0Exit; + static EVT_WDF_TIMER OnTimerExpire; + + // CLX callbacks + static EVT_SENSOR_DRIVER_START_SENSOR OnStart; + static EVT_SENSOR_DRIVER_STOP_SENSOR OnStop; + static EVT_SENSOR_DRIVER_GET_SUPPORTED_DATA_FIELDS OnGetSupportedDataFields; + static EVT_SENSOR_DRIVER_GET_PROPERTIES OnGetProperties; + static EVT_SENSOR_DRIVER_GET_DATA_FIELD_PROPERTIES OnGetDataFieldProperties; + static EVT_SENSOR_DRIVER_GET_DATA_INTERVAL OnGetDataInterval; + static EVT_SENSOR_DRIVER_SET_DATA_INTERVAL OnSetDataInterval; + static EVT_SENSOR_DRIVER_GET_DATA_THRESHOLDS OnGetDataThresholds; + static EVT_SENSOR_DRIVER_SET_DATA_THRESHOLDS OnSetDataThresholds; + static EVT_SENSOR_DRIVER_DEVICE_IO_CONTROL OnIoControl; + +private: + + NTSTATUS Initialize(_In_ WDFDEVICE Device, _In_ SENSOROBJECT SensorObj); + NTSTATUS GetData(); + +} FusionSensorDevice, *PFusionSensorDevice; + +// Set up accessor function to retrieve device context +WDF_DECLARE_CONTEXT_TYPE_WITH_NAME(FusionSensorDevice, GetFusionSensorContextFromSensorInstance); + +#define FusionSensorDevice_StepCount_Resolution (1) +#define FusionSensorDevice_StepCount_Minimum (0) +#define FusionSensorDevice_StepCount_Maximum (0xFFFFFFFF) diff --git a/sensors/Fusion/Driver.h b/sensors/Fusion/Driver.h new file mode 100644 index 00000000..46153894 --- /dev/null +++ b/sensors/Fusion/Driver.h @@ -0,0 +1,19 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved +// +//Abstract: +// +// This module contains the type definitions for the FusionSensor sample driver's +// driver callback class. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#pragma once + +WDF_EXTERN_C_START + +DRIVER_INITIALIZE DriverEntry; +EVT_WDF_DRIVER_UNLOAD OnDriverUnload; + +WDF_EXTERN_C_END
\ No newline at end of file diff --git a/sensors/Fusion/FusionSensor.ctl b/sensors/Fusion/FusionSensor.ctl new file mode 100644 index 00000000..54959227 --- /dev/null +++ b/sensors/Fusion/FusionSensor.ctl @@ -0,0 +1 @@ +0E08A3BA,F045,44EE,B00C,292C91C5F95A FusionSensorTraceGuid diff --git a/sensors/Fusion/FusionSensor.def b/sensors/Fusion/FusionSensor.def new file mode 100644 index 00000000..f49a363a --- /dev/null +++ b/sensors/Fusion/FusionSensor.def @@ -0,0 +1,6 @@ +; FusionSensor.def : Declares the module parameters. + +LIBRARY FusionSensor + +EXPORTS + diff --git a/sensors/Fusion/FusionSensor.inx b/sensors/Fusion/FusionSensor.inx Binary files differnew file mode 100644 index 00000000..1c562304 --- /dev/null +++ b/sensors/Fusion/FusionSensor.inx diff --git a/sensors/Fusion/FusionSensor.sln b/sensors/Fusion/FusionSensor.sln new file mode 100644 index 00000000..39ab0322 --- /dev/null +++ b/sensors/Fusion/FusionSensor.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 2013 +VisualStudioVersion = 12.0 +MinimumVisualStudioVersion = 12.0 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "FusionSensor", "FusionSensor.vcxproj", "{2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Release|Win32 = Release|Win32 + Debug|x64 = Debug|x64 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Debug|Win32.ActiveCfg = Debug|Win32 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Debug|Win32.Build.0 = Debug|Win32 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Release|Win32.ActiveCfg = Release|Win32 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Release|Win32.Build.0 = Release|Win32 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Debug|x64.ActiveCfg = Debug|x64 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Debug|x64.Build.0 = Debug|x64 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Release|x64.ActiveCfg = Release|x64 + {2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/sensors/Fusion/FusionSensor.vcxproj b/sensors/Fusion/FusionSensor.vcxproj new file mode 100644 index 00000000..f7a68e66 --- /dev/null +++ b/sensors/Fusion/FusionSensor.vcxproj @@ -0,0 +1,203 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="12.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Debug|x64"> + <Configuration>Debug</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|x64"> + <Configuration>Release</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <ProjectGuid>{2D98FAA6-F523-420D-9F3A-480CD7C3F9CA}</ProjectGuid> + <RootNamespace>$(MSBuildProjectName)</RootNamespace> + <UMDF_VERSION_MAJOR>2</UMDF_VERSION_MAJOR> + <Configuration Condition="'$(Configuration)' == ''">Debug</Configuration> + <Platform Condition="'$(Platform)' == ''">Win32</Platform> + <SampleGuid>{63E5D8E2-D2E3-4B7E-9F5A-78DA39FA9BED}</SampleGuid> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Label="Configuration" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <TargetVersion>Windows10</TargetVersion> + <UseDebugLibraries>False</UseDebugLibraries> + <DriverTargetPlatform>Universal</DriverTargetPlatform> + <DriverType>UMDF</DriverType> + <PlatformToolset>WindowsUserModeDriver10.0</PlatformToolset> + <ConfigurationType>DynamicLibrary</ConfigurationType> + </PropertyGroup> + <PropertyGroup Label="Configuration" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <TargetVersion>Windows10</TargetVersion> + <UseDebugLibraries>True</UseDebugLibraries> + <DriverTargetPlatform>Universal</DriverTargetPlatform> + <DriverType>UMDF</DriverType> + <PlatformToolset>WindowsUserModeDriver10.0</PlatformToolset> + <ConfigurationType>DynamicLibrary</ConfigurationType> + </PropertyGroup> + <PropertyGroup Label="Configuration" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <TargetVersion>Windows10</TargetVersion> + <UseDebugLibraries>False</UseDebugLibraries> + <DriverTargetPlatform>Universal</DriverTargetPlatform> + <DriverType>UMDF</DriverType> + <PlatformToolset>WindowsUserModeDriver10.0</PlatformToolset> + <ConfigurationType>DynamicLibrary</ConfigurationType> + </PropertyGroup> + <PropertyGroup Label="Configuration" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <TargetVersion>Windows10</TargetVersion> + <UseDebugLibraries>True</UseDebugLibraries> + <DriverTargetPlatform>Universal</DriverTargetPlatform> + <DriverType>UMDF</DriverType> + <PlatformToolset>WindowsUserModeDriver10.0</PlatformToolset> + <ConfigurationType>DynamicLibrary</ConfigurationType> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <PropertyGroup> + <OutDir>$(IntDir)</OutDir> + </PropertyGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" /> + </ImportGroup> + <ItemGroup Label="WrappedTaskItems"> + <ClCompile Include="client.cpp; device.cpp; driver.cpp; hardwaresimulator.cpp"> + <WppEnabled>true</WppEnabled> + <WppDllMacro>true</WppDllMacro> + <WppModuleName>FusionSensor</WppModuleName> + <WppScanConfigurationData>sensorstrace.h</WppScanConfigurationData> + </ClCompile> + <Inf Include="FusionSensor.inx"> + <Architecture>$(InfArch)</Architecture> + <SpecifyArchitecture>true</SpecifyArchitecture> + <CopyOutput>.\$(IntDir)\FusionSensor.inf</CopyOutput> + </Inf> + </ItemGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <TargetName>FusionSensor</TargetName> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <TargetName>FusionSensor</TargetName> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <TargetName>FusionSensor</TargetName> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <TargetName>FusionSensor</TargetName> + <CodeAnalysisRuleSet>AllRules.ruleset</CodeAnalysisRuleSet> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <ClCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <TreatWarningAsError>true</TreatWarningAsError> + <WarningLevel>Level4</WarningLevel> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + <ExceptionHandling> + </ExceptionHandling> + </ClCompile> + <Midl> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </Midl> + <ResourceCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </ResourceCompile> + <Link> + <AdditionalDependencies>%(AdditionalDependencies);$(SDK_LIB_PATH)\mincore.lib;$(SDK_LIB_PATH)\propsys.lib;$(SDK_LIB_PATH)\sensors\1.1\sensorscxstub.lib;$(SDK_LIB_PATH)\sensorsutils.lib</AdditionalDependencies> + <ModuleDefinitionFile>FusionSensor.def</ModuleDefinitionFile> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <ClCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <TreatWarningAsError>true</TreatWarningAsError> + <WarningLevel>Level4</WarningLevel> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + <ExceptionHandling>Sync</ExceptionHandling> + </ClCompile> + <Midl> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </Midl> + <ResourceCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </ResourceCompile> + <Link> + <AdditionalDependencies>%(AdditionalDependencies);$(SDK_LIB_PATH)\mincore.lib;$(SDK_LIB_PATH)\propsys.lib;$(SDK_LIB_PATH)\sensors\1.1\sensorscxstub.lib;$(SDK_LIB_PATH)\sensorsutils.lib</AdditionalDependencies> + <ModuleDefinitionFile>FusionSensor.def</ModuleDefinitionFile> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <TreatWarningAsError>true</TreatWarningAsError> + <WarningLevel>Level4</WarningLevel> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + <ExceptionHandling> + </ExceptionHandling> + </ClCompile> + <Midl> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </Midl> + <ResourceCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </ResourceCompile> + <Link> + <AdditionalDependencies>%(AdditionalDependencies);$(SDK_LIB_PATH)\mincore.lib;$(SDK_LIB_PATH)\propsys.lib;$(SDK_LIB_PATH)\sensors\1.1\sensorscxstub.lib;$(SDK_LIB_PATH)\sensorsutils.lib</AdditionalDependencies> + <ModuleDefinitionFile>FusionSensor.def</ModuleDefinitionFile> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <TreatWarningAsError>true</TreatWarningAsError> + <WarningLevel>Level4</WarningLevel> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + <ExceptionHandling>Sync</ExceptionHandling> + </ClCompile> + <Midl> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </Midl> + <ResourceCompile> + <PreprocessorDefinitions>%(PreprocessorDefinitions);_UNICODE;UNICODE;WPP_MACRO_USE_KM_VERSION_FOR_UM</PreprocessorDefinitions> + <AdditionalIncludeDirectories>%(AdditionalIncludeDirectories);$(SDK_INC_PATH);$(DDK_INC_PATH)\sensors\1.1;$(SDK_INC_PATH)\sensors\1.1</AdditionalIncludeDirectories> + </ResourceCompile> + <Link> + <AdditionalDependencies>%(AdditionalDependencies);$(SDK_LIB_PATH)\mincore.lib;$(SDK_LIB_PATH)\propsys.lib;$(SDK_LIB_PATH)\sensors\1.1\sensorscxstub.lib;$(SDK_LIB_PATH)\sensorsutils.lib</AdditionalDependencies> + <ModuleDefinitionFile>FusionSensor.def</ModuleDefinitionFile> + </Link> + </ItemDefinitionGroup> + <ItemGroup> + <Inf Exclude="@(Inf)" Include="*.inf" /> + <FilesToPackage Include="$(TargetPath)" Condition="'$(ConfigurationType)'=='Driver' or '$(ConfigurationType)'=='DynamicLibrary'" /> + </ItemGroup> + <ItemGroup> + <None Exclude="@(None)" Include="*.txt;*.htm;*.html" /> + <None Exclude="@(None)" Include="*.ico;*.cur;*.bmp;*.dlg;*.rct;*.gif;*.jpg;*.jpeg;*.wav;*.jpe;*.tiff;*.tif;*.png;*.rc2" /> + <None Exclude="@(None)" Include="*.def;*.bat;*.hpj;*.asmx" /> + </ItemGroup> + <ItemGroup> + <ClInclude Exclude="@(ClInclude)" Include="*.h;*.hpp;*.hxx;*.hm;*.inl;*.xsd" /> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> +</Project>
\ No newline at end of file diff --git a/sensors/Fusion/FusionSensor.vcxproj.Filters b/sensors/Fusion/FusionSensor.vcxproj.Filters new file mode 100644 index 00000000..a5ed8ef6 --- /dev/null +++ b/sensors/Fusion/FusionSensor.vcxproj.Filters @@ -0,0 +1,43 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx;*</Extensions> + <UniqueIdentifier>{47BC76FB-31D6-4043-A1F0-24C8EE1EC77D}</UniqueIdentifier> + </Filter> + <Filter Include="Header Files"> + <Extensions>h;hpp;hxx;hm;inl;inc;xsd</Extensions> + <UniqueIdentifier>{822384CB-A39F-4A21-8529-34A65C4CE181}</UniqueIdentifier> + </Filter> + <Filter Include="Resource Files"> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms;man;xml</Extensions> + <UniqueIdentifier>{C6D84098-A64B-429D-8EE2-D8ED8C5BF2FE}</UniqueIdentifier> + </Filter> + <Filter Include="Driver Files"> + <Extensions>inf;inv;inx;mof;mc;</Extensions> + <UniqueIdentifier>{A379ED39-5FF0-4DB2-A773-BDEA4FB77D87}</UniqueIdentifier> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="client.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="device.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="driver.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="hardwaresimulator.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <None Include="FusionSensor.def"> + <Filter>Source Files</Filter> + </None> + </ItemGroup> + <ItemGroup> + <Inf Include="FusionSensor.inx"> + <Filter>Driver Files</Filter> + </Inf> + </ItemGroup> +</Project>
\ No newline at end of file diff --git a/sensors/Fusion/HardwareSimulator.h b/sensors/Fusion/HardwareSimulator.h new file mode 100644 index 00000000..67e56436 --- /dev/null +++ b/sensors/Fusion/HardwareSimulator.h @@ -0,0 +1,60 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved +// +//Abstract: +// +// This module contains the type definitions for the FusionSensor sample driver +// hardware simulator. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#pragma once + +#include <windows.h> +#include <wdf.h> + +#include "SensorsTrace.h" +#include <SensorsCx.h> + +#include "Device.h" + +#define SIMULATOR_HARDWARE_INTERVAL_MS (1000) // 1 second interval in milliseconds + +typedef enum SIMULATOR_STATE +{ + SimulatorState_NotInitialized = 0, + SimulatorState_Initialized, + SimulatorState_Started +} SIMULATOR_STATE; + +typedef class HardwareSimulator +{ +private: + WDFTIMER m_Timer; + ULONG m_Index; + WDFWAITLOCK m_Lock; + SIMULATOR_STATE m_State; + WDFOBJECT m_SimulatorInstance; + BOOLEAN m_HasReset; + +public: + HardwareSimulator(); + ~HardwareSimulator(); + + // WDF callbacks + static EVT_WDF_TIMER OnTimerExpire; + + static NTSTATUS Initialize(_In_ WDFDEVICE Device, _Out_ WDFOBJECT *SimulatorInstance); + NTSTATUS Cleanup(); + NTSTATUS Start(); + NTSTATUS Stop(); + NTSTATUS GetSample(_Out_ FusionSensorSample *Sample); + +private: + NTSTATUS InitializeInternal(_In_ WDFOBJECT SimulatorInstance); +} HardwareSimulator, *PHardwareSimulator; + + +// Set up accessor function to retrieve device context +WDF_DECLARE_CONTEXT_TYPE_WITH_NAME(HardwareSimulator, GetHardwareSimulatorContextFromInstance); diff --git a/sensors/Fusion/SensorsTrace.h b/sensors/Fusion/SensorsTrace.h new file mode 100644 index 00000000..8b3b8769 --- /dev/null +++ b/sensors/Fusion/SensorsTrace.h @@ -0,0 +1,102 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved +// +//Abstract: +// +// Header file for the debug tracing related function defintions and macros. +// +//Environment: +// +// User mode + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +// Define the tracing flags. +// +// Tracing GUID - 0E08A3BA-F045-44EE-B00C-292C91C5F95A + +#define WPP_CONTROL_GUIDS \ + WPP_DEFINE_CONTROL_GUID( \ + FusionSensorTraceGuid, (0E08A3BA,F045,44EE,B00C,292C91C5F95A), \ + WPP_DEFINE_BIT(EntryExit) \ + WPP_DEFINE_BIT(DataFlow) \ + WPP_DEFINE_BIT(Verbose) \ + WPP_DEFINE_BIT(Information) \ + WPP_DEFINE_BIT(Warning) \ + WPP_DEFINE_BIT(Error) \ + WPP_DEFINE_BIT(Fatal) \ + WPP_DEFINE_BIT(DriverStatus) \ + ) + +#define WPP_FLAG_LEVEL_LOGGER(flag, level) WPP_LEVEL_LOGGER(flag) + +#define WPP_FLAG_LEVEL_ENABLED(flag, level) (WPP_LEVEL_ENABLED(flag) && WPP_CONTROL(WPP_BIT_ ## flag).Level >= level) + +#define WPP_LEVEL_FLAGS_LOGGER(level,flags) WPP_LEVEL_LOGGER(flags) + +#define WPP_LEVEL_FLAGS_ENABLED(level, flags) (WPP_LEVEL_ENABLED(flags) && WPP_CONTROL(WPP_BIT_ ## flags).Level >= level) + +// This comment block is scanned by the trace preprocessor to define our +// Trace function. +// +// begin_wpp config +// +// FUNC TraceEvents(LEVEL, FLAGS, MSG, ...); +// +// FUNC TraceFatal{LEVEL=TRACE_LEVEL_FATAL,FLAGS=Fatal}(MSG,...); +// FUNC TraceError{LEVEL=TRACE_LEVEL_ERROR,FLAGS=Error}(MSG,...); +// FUNC TraceWarning{LEVEL=TRACE_LEVEL_WARNING,FLAGS=Warning}(MSG,...); +// FUNC TraceInformation{LEVEL=TRACE_LEVEL_INFORMATION,FLAGS=Information}(MSG,...); +// FUNC TraceVerbose{LEVEL=TRACE_LEVEL_VERBOSE,FLAGS=Verbose}(MSG,...); +// FUNC TracePerformance{PERF=DUMMY,LEVEL=TRACE_LEVEL_PERF}(FLAGS,MSG,...); +// +// FUNC TraceData{LEVEL=TRACE_LEVEL_VERBOSE,FLAGS=DataFlow}(MSG,...); +// +// FUNC TraceDriverStatus{LEVEL=TRACE_LEVEL_INFORMATION,FLAGS=DriverStatus}(MSG,...); +// +// end_wpp + + + +// SENSOR ------------------------------------------------------------------------------------------------ + +// MACRO: SENSOR_FunctionEnter +// +// begin_wpp config +// USEPREFIX (SENSOR_FunctionEnter, "%!STDPREFIX! SENSOR %!FUNC! FunctionEnter"); +// FUNC SENSOR_FunctionEnter{LEVEL=TRACE_LEVEL_VERBOSE,FLAGS=EntryExit}(...); +// end_wpp + + +// MACRO: SENSOR_FunctionExit +// +// begin_wpp config +// USEPREFIX (SENSOR_FunctionExit, "%!STDPREFIX! SENSOR %!FUNC! FunctionExit: %!STATUS!", __status); +// FUNC SENSOR_FunctionExit{LEVEL=TRACE_LEVEL_VERBOSE,FLAGS=EntryExit}(SENSOREXIT); +// end_wpp +#define WPP_LEVEL_FLAGS_SENSOREXIT_ENABLED(LEVEL, FLAGS, status) WPP_LEVEL_FLAGS_ENABLED(LEVEL, FLAGS) +#define WPP_LEVEL_FLAGS_SENSOREXIT_LOGGER(LEVEL, FLAGS, status) WPP_LEVEL_FLAGS_LOGGER(LEVEL, FLAGS) + +#define WPP_LEVEL_FLAGS_SENSOREXIT_PRE(LEVEL, FLAGS, status) { \ + NTSTATUS __status = status; +#define WPP_LEVEL_FLAGS_SENSOREXIT_POST(LEVEL, FLAGS, status) /*TraceMessage()*/; \ + } + + + +// WPP Recorder ------------------------------------------------------------------------------------------- +// +// The following two macros are required to enable WPP Recorder functionality for clients of the Sensor Class Extension +// +#define WPP_RECORDER_LEVEL_FLAGS_SENSOREXIT_FILTER(LEVEL, FLAGS, status) WPP_RECORDER_LEVEL_FLAGS_FILTER(LEVEL, FLAGS) +#define WPP_RECORDER_LEVEL_FLAGS_SENSOREXIT_ARGS(LEVEL, FLAGS, status) WPP_RECORDER_LEVEL_FLAGS_ARGS(LEVEL, FLAGS) + + +#ifdef __cplusplus +} +#endif + + diff --git a/sensors/Fusion/client.cpp b/sensors/Fusion/client.cpp new file mode 100644 index 00000000..686b4197 --- /dev/null +++ b/sensors/Fusion/client.cpp @@ -0,0 +1,789 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved. +// +//Abstract: +// +// This module contains the implementation of driver callback function +// from clx to FusionSensor. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#include "Device.h" +#include "HardwareSimulator.h" + +#include <timeapi.h> +#include <Intsafe.h> + +#include "Client.tmh" + +// This routine is called by worker thread to read a single sample, compare threshold +// and push it back to CLX. It simulates hardware thresholding by only generating data +// when the change of data is greater than threshold. +NTSTATUS +FusionSensorDevice::GetData( +) +{ + // TODO: Remove the HardwareSimulator code in your final driver. The HardwareSimulator is only used for the purpose of demonstrating how sensor driver samples work. + PHardwareSimulator pSimulator = GetHardwareSimulatorContextFromInstance(m_SimulatorInstance); + BOOLEAN DataReady = FALSE; + NTSTATUS Status = STATUS_SUCCESS; + FusionSensorSample Sample = {}; + + SENSOR_FunctionEnter(); + + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + goto Exit; + } + + // TODO: In this case, we are calling into the HardwareSimulator code to get some simulated data. + // In a real driver (which either communicates with real hardware or some other drivers), this call should be replaced by some + // logic to get data comming from the hardware/other drivers. The "Sample" variable is expected to contain actual data from here on. + Status = pSimulator->GetSample(&Sample); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! GetSample failed %!STATUS!", Status); + goto Exit; + } + + if (FALSE != m_FirstSample) + { + Status = GetPerformanceTime(&m_StartTime); + if (!NT_SUCCESS(Status)) + { + m_StartTime = 0; + TraceError("FUS %!FUNC! GetPerformanceTime failed %!STATUS!", Status); + } + + m_SampleCount = 0; + + DataReady = TRUE; + } + else + { + // Compare the change of data to threshold, and only push the data back to + // clx if the change exceeds threshold. + + CQUATERNION Quaternion = Sample.Quaternion; + FLOAT RotationAngle = Quaternion.ToAngleAxis(nullptr) * RadToDegRatio; + + CQUATERNION LastQuaternion = m_LastSample.Quaternion; + FLOAT LastRotationAngle = LastQuaternion.ToAngleAxis(nullptr) * RadToDegRatio; + + TraceData("FUS %!FUNC! Rotation Angle: new=%f, old=%f", RotationAngle, LastRotationAngle); + + if ((abs(RotationAngle - LastRotationAngle) >= m_CachedThresholds.RotationAngle)) + { + DataReady = TRUE; + } + } + + if (FALSE != DataReady) + { + // update last sample + m_LastSample.Quaternion = Sample.Quaternion; + m_LastSample.Accuracy = Sample.Accuracy; + m_LastSample.DeclinationAngle = Sample.DeclinationAngle; + + // push to clx + InitPropVariantFromFileTime(&m_LastSample.Timestamp, &(m_pData->List[FUSIONSENSOR_DATA_TIMESTAMP].Value)); + InitPropVariantFromFloat(m_LastSample.Quaternion.W, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_W].Value)); + InitPropVariantFromFloat(m_LastSample.Quaternion.X, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_X].Value)); + InitPropVariantFromFloat(m_LastSample.Quaternion.Y, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Y].Value)); + InitPropVariantFromFloat(m_LastSample.Quaternion.Z, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Z].Value)); + InitPropVariantFromUInt32(m_LastSample.Accuracy, &(m_pData->List[FUSIONSENSOR_DATA_ACCURACY].Value)); + InitPropVariantFromFloat(m_LastSample.DeclinationAngle, &(m_pData->List[FUSIONSENSOR_DATA_DECLINATION_ANGLE].Value)); + + + SensorsCxSensorDataReady(m_SensorInstance, m_pData); + m_FirstSample = FALSE; + } + else + { + Status = STATUS_DATA_NOT_ACCEPTED; + TraceInformation("FUS %!FUNC! Data did NOT meet the threshold"); + } + + SENSOR_FunctionExit(Status); + +Exit: + return Status; +} + + + +// This callback is called when interval wait time has expired and driver is ready +// to collect new sample. The callback reads current value, compare value to threshold, +// pushes it up to CLX framework, and schedule next wake up time. +VOID +FusionSensorDevice::OnTimerExpire( + _In_ WDFTIMER Timer // WDF timer object + ) +{ + PFusionSensorDevice pDevice = nullptr; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + pDevice = GetFusionSensorContextFromSensorInstance(WdfTimerGetParentObject(Timer)); + if (nullptr == pDevice) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetFusionSensorContextFromSensorInstance failed %!STATUS!", Status); + goto Exit; + } + + // Get data and push to clx + WdfWaitLockAcquire(pDevice->m_Lock, NULL); + Status = pDevice->GetData(); + if (!NT_SUCCESS(Status) && Status != STATUS_DATA_NOT_ACCEPTED) + { + TraceError("FUS %!FUNC! GetData Failed %!STATUS!", Status); + } + WdfWaitLockRelease(pDevice->m_Lock); + + // Schedule next wake up time + if (FALSE != pDevice->m_PoweredOn && + FALSE != pDevice->m_Started) + { + LONGLONG WaitTimeHundredNanoseconds = 0; // in unit of 100ns + + if (0 == pDevice->m_StartTime) + { + // in case we fail to get sensor start time, use static wait time + WaitTimeHundredNanoseconds = WDF_REL_TIMEOUT_IN_MS(pDevice->m_Interval); + } + else + { + ULONG CurrentTimeMs = 0; + + // dynamically calculate wait time to avoid jitter + Status = GetPerformanceTime (&CurrentTimeMs); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! GetPerformanceTime %!STATUS!", Status); + WaitTimeHundredNanoseconds = WDF_REL_TIMEOUT_IN_MS(pDevice->m_Interval); + } + else + { + pDevice->m_SampleCount++; + if (CurrentTimeMs > (pDevice->m_StartTime + (pDevice->m_Interval * (pDevice->m_SampleCount + 1)))) + { + // If we skipped two or more beats, reschedule the timer with a zero due time to catch up on missing samples + WaitTimeHundredNanoseconds = 0; + } + else + { + WaitTimeHundredNanoseconds = (pDevice->m_StartTime + + (pDevice->m_Interval * (pDevice->m_SampleCount + 1))) - CurrentTimeMs; + } + WaitTimeHundredNanoseconds = WDF_REL_TIMEOUT_IN_MS(WaitTimeHundredNanoseconds); + } + } + WdfTimerStart(pDevice->m_Timer, WaitTimeHundredNanoseconds); + } + +Exit: + + SENSOR_FunctionExit(Status); +} + + + +// Called by Sensor CLX to begin continuously sampling the sensor. +NTSTATUS +FusionSensorDevice::OnStart( + _In_ SENSOROBJECT SensorInstance // Sensor device object + ) +{ + PHardwareSimulator pSimulator = nullptr; + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Sensor(0x%p) parameter is invalid. Failed %!STATUS!", SensorInstance, Status); + goto Exit; + } + + // Get the simulator context + pSimulator = GetHardwareSimulatorContextFromInstance(pDevice->m_SimulatorInstance); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + } + + if (NT_SUCCESS(Status)) + { + // Start the simulator + pSimulator->Start(); + + pDevice->m_FirstSample = TRUE; + + // Start polling + + pDevice->m_Started = TRUE; + + InitPropVariantFromUInt32(SensorState_Active, + &(pDevice->m_pProperties->List[SENSOR_PROPERTY_STATE].Value)); + + // Start the sample polling timer. + // + // Note: The polling timer is configured to allow for the first sample to be reported immediately. + // Some hardware may want to delay the first sample report a little to account for hardware start time. + WdfTimerStart(pDevice->m_Timer, WDF_REL_TIMEOUT_IN_MS(FusionSensor_Default_MinDataInterval_Ms)); + } +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// Called by Sensor CLX to stop continuously sampling the sensor. +NTSTATUS +FusionSensorDevice::OnStop( + _In_ SENSOROBJECT SensorInstance // Sensor device object + ) +{ + PHardwareSimulator pSimulator = nullptr; + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Sensor(0x%p) parameter is invalid. Failed %!STATUS!", SensorInstance, Status); + goto Exit; + } + + // Stop polling + pDevice->m_Started = FALSE; + + // Waiting for the callback to complete, then stopping the timer + WdfTimerStop(pDevice->m_Timer, TRUE); + + InitPropVariantFromUInt32(SensorState_Idle, + &(pDevice->m_pProperties->List[SENSOR_PROPERTY_STATE].Value)); + + // Stop the simulator + pSimulator = GetHardwareSimulatorContextFromInstance(pDevice->m_SimulatorInstance); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + goto Exit; + } + + pSimulator->Stop(); + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + +// Called by Sensor CLX to get supported data fields. The typical usage is to call +// this function once with buffer pointer as NULL to acquire the required size +// for the buffer, allocate buffer, then call the function again to retrieve +// sensor information. +NTSTATUS +FusionSensorDevice::OnGetSupportedDataFields( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _Inout_opt_ PSENSOR_PROPERTY_LIST pFields, // Pointer to a list of supported properties + _Out_ PULONG pSize // Number of bytes for the list of supported properties + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice || nullptr == pSize) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Invalid parameters! %!STATUS!", Status); + goto Exit; + } + + if (nullptr == pFields) + { + // Just return size + *pSize = pDevice->m_pSupportedDataFields->AllocatedSizeInBytes; + } + else + { + if (pFields->AllocatedSizeInBytes < pDevice->m_pSupportedDataFields->AllocatedSizeInBytes) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Buffer is too small. Failed %!STATUS!", Status); + goto Exit; + } + + // Fill out data + Status = PropertiesListCopy(pFields, pDevice->m_pSupportedDataFields); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropertiesListCopy failed %!STATUS!", Status); + goto Exit; + } + + *pSize = pDevice->m_pSupportedDataFields->AllocatedSizeInBytes; + } + +Exit: + if (!NT_SUCCESS(Status)) + { + *pSize = 0; + } + SENSOR_FunctionExit(Status); + return Status; +} + + + +// Called by Sensor CLX to get sensor properties. The typical usage is to call +// this function once with buffer pointer as NULL to acquire the required size +// for the buffer, allocate buffer, then call the function again to retrieve +// sensor information. +NTSTATUS +FusionSensorDevice::OnGetProperties( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _Inout_opt_ PSENSOR_COLLECTION_LIST pProperties, // Pointer to a list of sensor properties + _Out_ PULONG pSize // Number of bytes for the list of sensor properties + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice || nullptr == pSize) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Invalid parameters! %!STATUS!", Status); + goto Exit; + } + + if (nullptr == pProperties) + { + // Just return size + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pProperties); + } + else + { + if (pProperties->AllocatedSizeInBytes < + CollectionsListGetMarshalledSize(pDevice->m_pProperties)) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Buffer is too small. Failed %!STATUS!", Status); + goto Exit; + } + + // Fill out all data + Status = CollectionsListCopyAndMarshall(pProperties, pDevice->m_pProperties); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! CollectionsListCopyAndMarshall failed %!STATUS!", Status); + goto Exit; + } + + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pProperties); + } + +Exit: + if (!NT_SUCCESS(Status)) + { + *pSize = 0; + } + SENSOR_FunctionExit(Status); + return Status; +} + + +// Called by Sensor CLX to get data field properties. The typical usage is to call +// this function once with buffer pointer as NULL to acquire the required size +// for the buffer, allocate buffer, then call the function again to retrieve +// sensor information. +NTSTATUS +FusionSensorDevice::OnGetDataFieldProperties( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _In_ const PROPERTYKEY *DataField, // Pointer to the propertykey of requested property + _Inout_opt_ PSENSOR_COLLECTION_LIST pProperties, // Pointer to a list of sensor properties + _Out_ PULONG pSize // Number of bytes for the list of sensor properties + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice || nullptr == pSize || nullptr == DataField) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Invalid parameters! %!STATUS!", Status); + goto Exit; + } + + + if ((*DataField == PKEY_SensorData_LinearAccelerationX_Gs) || + (*DataField == PKEY_SensorData_LinearAccelerationY_Gs) || + (*DataField == PKEY_SensorData_LinearAccelerationZ_Gs)) + { + // Linear Acceleration + if (nullptr == pProperties) + { + // Just return size + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pAccDataFieldProperties); + } + else + { + if (pProperties->AllocatedSizeInBytes < + CollectionsListGetMarshalledSize(pDevice->m_pAccDataFieldProperties)) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Buffer is too small. Failed %!STATUS!", Status); + goto Exit; + } + + // Fill out all data + Status = CollectionsListCopyAndMarshall(pProperties, pDevice->m_pAccDataFieldProperties); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! CollectionsListCopyAndMarshall failed %!STATUS!", Status); + goto Exit; + } + + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pAccDataFieldProperties); + } + } + else if ((*DataField == PKEY_SensorData_CorrectedAngularVelocityX_DegreesPerSecond) || + (*DataField == PKEY_SensorData_CorrectedAngularVelocityY_DegreesPerSecond) || + (*DataField == PKEY_SensorData_CorrectedAngularVelocityZ_DegreesPerSecond)) + { + // Rotation Rate + if (nullptr == pProperties) + { + // Just return size + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pGyrDataFieldProperties); + } + else + { + if (pProperties->AllocatedSizeInBytes < + CollectionsListGetMarshalledSize(pDevice->m_pGyrDataFieldProperties)) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Buffer is too small. Failed %!STATUS!", Status); + goto Exit; + } + + // Fill out all data + Status = CollectionsListCopyAndMarshall(pProperties, pDevice->m_pGyrDataFieldProperties); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! CollectionsListCopyAndMarshall failed %!STATUS!", Status); + goto Exit; + } + + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pGyrDataFieldProperties); + } + } + else + { + Status = STATUS_NOT_SUPPORTED; + TraceError("FUS %!FUNC! Fusion sensor does NOT have properties for this data field. Failed %!STATUS!", Status); + goto Exit; + } + +Exit: + if (!NT_SUCCESS(Status)) + { + *pSize = 0; + } + SENSOR_FunctionExit(Status); + return Status; +} + + + +// Called by Sensor CLX to get sampling rate of the sensor. +NTSTATUS +FusionSensorDevice::OnGetDataInterval( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _Out_ PULONG DataRateMs // Sampling rate in ms + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Sensor(0x%p) parameter is invalid. Failed %!STATUS!", SensorInstance, Status); + goto Exit; + } + + if (nullptr == DataRateMs) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! DataRateMs(0x%p) parameter is invalid. Failed %!STATUS!", DataRateMs, Status); + goto Exit; + } + + *DataRateMs = pDevice->m_Interval; + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// Called by Sensor CLX to set sampling rate of the sensor. +NTSTATUS +FusionSensorDevice::OnSetDataInterval( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _In_ ULONG DataRateMs // Sampling rate in ms + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Sensor(0x%p) parameter is invalid. Failed %!STATUS!", SensorInstance, Status); + goto Exit; + } + + if (FusionSensor_Default_MinDataInterval_Ms > DataRateMs) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! DataRateMs(%d) parameter is smaller than the minimum data interval. Failed %!STATUS!", DataRateMs, Status); + goto Exit; + } + + pDevice->m_Interval = DataRateMs; + + // reschedule sample to return as soon as possible if it's started + if (FALSE != pDevice->m_Started) + { + pDevice->m_Started = FALSE; + WdfTimerStop(pDevice->m_Timer, TRUE); + + pDevice->m_Started = TRUE; + pDevice->m_FirstSample = TRUE; + WdfTimerStart(pDevice->m_Timer, WDF_REL_TIMEOUT_IN_MS(FusionSensor_Default_MinDataInterval_Ms)); + } + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// Called by Sensor CLX to get data thresholds. The typical usage is to call +// this function once with buffer pointer as NULL to acquire the required size +// for the buffer, allocate buffer, then call the function again to retrieve +// sensor information. +NTSTATUS +FusionSensorDevice::OnGetDataThresholds( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _Inout_opt_ PSENSOR_COLLECTION_LIST pThresholds, // Pointer to a list of sensor thresholds + _Out_ PULONG pSize // Number of bytes for the list of sensor thresholds + ) +{ + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == pDevice || nullptr == pSize) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Invalid parameters! %!STATUS!", Status); + goto Exit; + } + + if (nullptr == pThresholds) + { + // Just return size + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pThresholds); + } + else + { + if (pThresholds->AllocatedSizeInBytes < + CollectionsListGetMarshalledSize(pDevice->m_pThresholds)) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Buffer is too small. Failed %!STATUS!", Status); + goto Exit; + } + + // Fill out all data + Status = CollectionsListCopyAndMarshall(pThresholds, pDevice->m_pThresholds); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! CollectionsListCopyAndMarshall failed %!STATUS!", Status); + goto Exit; + } + + *pSize = CollectionsListGetMarshalledSize(pDevice->m_pThresholds); + } + +Exit: + if (!NT_SUCCESS(Status)) + { + *pSize = 0; + } + + SENSOR_FunctionExit(Status); + + return Status; +} + + + +// Called by Sensor CLX to set data thresholds. +NTSTATUS +FusionSensorDevice::OnSetDataThresholds( + _In_ SENSOROBJECT SensorInstance, // Sensor device object + _In_ PSENSOR_COLLECTION_LIST pThresholds // Pointer to a list of sensor thresholds + ) +{ + ULONG Element; + BOOLEAN IsLocked = FALSE; + PFusionSensorDevice pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (pDevice == nullptr) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! Sensor(0x%p) parameter is invalid. Failed %!STATUS!", SensorInstance, Status); + goto Exit; + } + + WdfWaitLockAcquire(pDevice->m_Lock, NULL); + IsLocked = TRUE; + + for (Element = 0; Element < pThresholds->Count; Element++) + { + Status = PropKeyFindKeySetPropVariant(pDevice->m_pThresholds, + &(pThresholds->List[Element].Key), + TRUE, + &(pThresholds->List[Element].Value)); + if (!NT_SUCCESS(Status)) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! FusionSensor driver does NOT have threshold for this data field. Failed %!STATUS!", Status); + goto Exit; + } + } + + // Get data thresholds + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_RotationAngle_Degrees, + &(pDevice->m_CachedThresholds.RotationAngle)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for rotation angle failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_LinearAccelerationX_Gs, + &(pDevice->m_CachedThresholds.LinearAcceleration.X)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for linear acceleration X failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_LinearAccelerationY_Gs, + &(pDevice->m_CachedThresholds.LinearAcceleration.Y)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for linear acceleration Y failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_LinearAccelerationZ_Gs, + &(pDevice->m_CachedThresholds.LinearAcceleration.Z)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for linear acceleration Z failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_CorrectedAngularVelocityX_DegreesPerSecond, + &(pDevice->m_CachedThresholds.RotationRate.X)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for rotation rate X failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_CorrectedAngularVelocityY_DegreesPerSecond, + &(pDevice->m_CachedThresholds.RotationRate.Y)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for rotation rate Y failed! %!STATUS!", Status); + goto Exit; + } + + Status = PropKeyFindKeyGetFloat(pDevice->m_pThresholds, + &PKEY_SensorData_CorrectedAngularVelocityZ_DegreesPerSecond, + &(pDevice->m_CachedThresholds.RotationRate.Z)); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! PropKeyFindKeyGetFloat for rotation rate Z failed! %!STATUS!", Status); + goto Exit; + } + +Exit: + if (FALSE != IsLocked) + { + WdfWaitLockRelease(pDevice->m_Lock); + IsLocked = FALSE; + } + SENSOR_FunctionExit(Status); + return Status; +} + + +// Called by Sensor CLX to handle IOCTLs that clx does not support +NTSTATUS +FusionSensorDevice::OnIoControl( + _In_ SENSOROBJECT /*SensorInstance*/, // WDF queue object + _In_ WDFREQUEST /*Request*/, // WDF request object + _In_ size_t /*OutputBufferLength*/, // number of bytes to retrieve from output buffer + _In_ size_t /*InputBufferLength*/, // number of bytes to retrieve from input buffer + _In_ ULONG /*IoControlCode*/ // IOCTL control code + ) +{ + NTSTATUS Status = STATUS_NOT_SUPPORTED; + + SENSOR_FunctionEnter(); + + SENSOR_FunctionExit(Status); + return Status; +}
\ No newline at end of file diff --git a/sensors/Fusion/device.cpp b/sensors/Fusion/device.cpp new file mode 100644 index 00000000..0ac6e042 --- /dev/null +++ b/sensors/Fusion/device.cpp @@ -0,0 +1,729 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved. +// +//Abstract: +// +// This module contains the implementation of WDF callback functions +// for FusionSensor driver. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#include "Device.h" +#include "HardwareSimulator.h" + +#include "Device.tmh" + +// FusionSensor Unique ID +// {997335D7-A71C-4C89-9D95-58EAEF917C6A} +// +// TODO: The unique ID below must be set per sensor. A different GUID must be provided for each sensor. Please generate a new GUID. +DEFINE_GUID(GUID_FusionSensorDevice_UniqueID, + 0x997335d7, 0xa71c, 0x4c89, 0x9d, 0x95, 0x58, 0xea, 0xef, 0x91, 0x7c, 0x6a); + + +// This routine initializes the sensor to its default properties +NTSTATUS +FusionSensorDevice::Initialize( + _In_ WDFDEVICE Device, // WDFDEVICE object + _In_ SENSOROBJECT SensorInstance // SENSOROBJECT for each sensor instance + ) +{ + ULONG Size = 0; + WDF_OBJECT_ATTRIBUTES MemoryAttributes; + WDFMEMORY MemoryHandle = NULL; + FILETIME Time = {}; + WDF_OBJECT_ATTRIBUTES TimerAttributes; + WDF_TIMER_CONFIG TimerConfig; + NTSTATUS Status = STATUS_SUCCESS; + PHardwareSimulator pSimulator = nullptr; + + SENSOR_FunctionEnter(); + + // Store device and instance + m_FxDevice = Device; + m_SensorInstance = SensorInstance; + m_Started = FALSE; + + // TODO: Remove the HardwareSimulator code in your final driver. The HardwareSimulator is only used for the purpose of demonstrating how sensor driver samples work. + // Initialize the FusionSensor simulator + Status = HardwareSimulator::Initialize(Device, &m_SimulatorInstance); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! HardwareSimulator::Initialize failed %!STATUS!", Status); + goto Exit; + } + + pSimulator = GetHardwareSimulatorContextFromInstance(m_SimulatorInstance); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + goto Exit; + } + + // Create Lock + Status = WdfWaitLockCreate(WDF_NO_OBJECT_ATTRIBUTES, &m_Lock); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! WdfWaitLockCreate failed %!STATUS!", Status); + goto Exit; + } + + // Create timer object for polling sensor samples + WDF_TIMER_CONFIG_INIT(&TimerConfig, FusionSensorDevice::OnTimerExpire); + WDF_OBJECT_ATTRIBUTES_INIT(&TimerAttributes); + TimerAttributes.ParentObject = SensorInstance; + TimerAttributes.ExecutionLevel = WdfExecutionLevelPassive; + + Status = WdfTimerCreate(&TimerConfig, &TimerAttributes, &m_Timer); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! WdfTimerCreate failed %!STATUS!", Status); + goto Exit; + } + + // + // Sensor Enumeration Properties + // + { + Size = SENSOR_COLLECTION_LIST_SIZE(SENSOR_ENUMERATION_PROPERTIES_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pEnumerationProperties); + if (!NT_SUCCESS(Status) || m_pEnumerationProperties == nullptr) + { + TraceError("FUS %!FUNC! WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pEnumerationProperties, Size); + m_pEnumerationProperties->Count = SENSOR_ENUMERATION_PROPERTIES_COUNT; + + m_pEnumerationProperties->List[SENSOR_TYPE_GUID].Key = DEVPKEY_Sensor_Type; + InitPropVariantFromCLSID(GUID_SensorType_Orientation, + &(m_pEnumerationProperties->List[SENSOR_TYPE_GUID].Value)); + + m_pEnumerationProperties->List[SENSOR_MANUFACTURER].Key = DEVPKEY_Sensor_Manufacturer; + InitPropVariantFromString(L"TODO-Set-Manufacturer", + &(m_pEnumerationProperties->List[SENSOR_MANUFACTURER].Value)); + + m_pEnumerationProperties->List[SENSOR_MODEL].Key = DEVPKEY_Sensor_Model; + InitPropVariantFromString(L"Sample fusion sensor", + &(m_pEnumerationProperties->List[SENSOR_MODEL].Value)); + + m_pEnumerationProperties->List[SENSOR_PERSISTENT_UNIQUEID].Key = DEVPKEY_Sensor_PersistentUniqueId; + InitPropVariantFromCLSID(GUID_FusionSensorDevice_UniqueID, + &(m_pEnumerationProperties->List[SENSOR_PERSISTENT_UNIQUEID].Value)); + + m_pEnumerationProperties->List[SENSOR_ISPRIMARY].Key = DEVPKEY_Sensor_IsPrimary; + InitPropVariantFromBoolean(FALSE, + &(m_pEnumerationProperties->List[SENSOR_ISPRIMARY].Value)); + } + + // + // Supported Data-Fields + // + { + Size = SENSOR_PROPERTY_LIST_SIZE(FUSIONSENSOR_DATA_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pSupportedDataFields); + if (!NT_SUCCESS(Status) || m_pSupportedDataFields == nullptr) + { + TraceError("FUS %!FUNC! WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_PROPERTY_LIST_INIT(m_pSupportedDataFields, Size); + m_pSupportedDataFields->Count = FUSIONSENSOR_DATA_COUNT; + + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_TIMESTAMP] = PKEY_SensorData_Timestamp; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_QUATERNION_W] = PKEY_SensorData_QuaternionW; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_QUATERNION_X] = PKEY_SensorData_QuaternionX; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_QUATERNION_Y] = PKEY_SensorData_QuaternionY; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_QUATERNION_Z] = PKEY_SensorData_QuaternionZ; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_ACCURACY] = PKEY_SensorData_MagnetometerAccuracy; + m_pSupportedDataFields->List[FUSIONSENSOR_DATA_DECLINATION_ANGLE] = PKEY_SensorData_DeclinationAngle_Degrees; + } + + // + // Data + // + { + Size = SENSOR_COLLECTION_LIST_SIZE(FUSIONSENSOR_DATA_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pData); + if (!NT_SUCCESS(Status) || m_pData == nullptr) + { + TraceError("FUS %!FUNC! WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pData, Size); + m_pData->Count = FUSIONSENSOR_DATA_COUNT; + + m_pData->List[FUSIONSENSOR_DATA_TIMESTAMP].Key = PKEY_SensorData_Timestamp; + GetSystemTimePreciseAsFileTime(&Time); + InitPropVariantFromFileTime(&Time, &(m_pData->List[FUSIONSENSOR_DATA_TIMESTAMP].Value)); + + m_pData->List[FUSIONSENSOR_DATA_QUATERNION_W].Key = PKEY_SensorData_QuaternionW; + InitPropVariantFromFloat(0.0, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_W].Value)); + + m_pData->List[FUSIONSENSOR_DATA_QUATERNION_X].Key = PKEY_SensorData_QuaternionX; + InitPropVariantFromFloat(0.0, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_X].Value)); + + m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Y].Key = PKEY_SensorData_QuaternionY; + InitPropVariantFromFloat(0.0, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Y].Value)); + + m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Z].Key = PKEY_SensorData_QuaternionZ; + InitPropVariantFromFloat(0.0, &(m_pData->List[FUSIONSENSOR_DATA_QUATERNION_Z].Value)); + + m_pData->List[FUSIONSENSOR_DATA_ACCURACY].Key = PKEY_SensorData_MagnetometerAccuracy; + InitPropVariantFromUInt32(Unknown, &(m_pData->List[FUSIONSENSOR_DATA_ACCURACY].Value)); + + // Declination angle is optional, it will be computed by the system if not present. + m_pData->List[FUSIONSENSOR_DATA_DECLINATION_ANGLE].Key = PKEY_SensorData_DeclinationAngle_Degrees; + InitPropVariantFromFloat(0.0, &(m_pData->List[FUSIONSENSOR_DATA_DECLINATION_ANGLE].Value)); + } + + // + // Sensor Properties + // + { + m_Interval = FusionSensor_Default_DataInterval; + + Size = SENSOR_COLLECTION_LIST_SIZE(SENSOR_PROPERTIES_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pProperties); + if (!NT_SUCCESS(Status) || m_pProperties == nullptr) + { + TraceError("FUS %!FUNC! WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pProperties, Size); + m_pProperties->Count = SENSOR_PROPERTIES_COUNT; + + m_pProperties->List[SENSOR_PROPERTY_STATE].Key = PKEY_Sensor_State; + InitPropVariantFromUInt32(SensorState_Initializing, + &(m_pProperties->List[SENSOR_PROPERTY_STATE].Value)); + + m_pProperties->List[SENSOR_PROPERTY_MIN_INTERVAL].Key = PKEY_Sensor_MinimumDataInterval_Ms; + InitPropVariantFromUInt32(FusionSensor_Default_MinDataInterval_Ms, + &(m_pProperties->List[SENSOR_PROPERTY_MIN_INTERVAL].Value)); + + m_pProperties->List[SENSOR_PROPERTY_MAX_DATAFIELDSIZE].Key = PKEY_Sensor_MaximumDataFieldSize_Bytes; + InitPropVariantFromUInt32(CollectionsListGetMarshalledSize(m_pData), + &(m_pProperties->List[SENSOR_PROPERTY_MAX_DATAFIELDSIZE].Value)); + + m_pProperties->List[SENSOR_PROPERTY_SENSOR_TYPE].Key = PKEY_Sensor_Type; + InitPropVariantFromCLSID(GUID_SensorType_Orientation, + &(m_pProperties->List[SENSOR_PROPERTY_SENSOR_TYPE].Value)); + + m_pProperties->List[SENSOR_PROPERTY_USE_GYRO].Key = PKEY_OrientationSensor_GyroscopeUsed; + InitPropVariantFromBoolean(FALSE, &(m_pProperties->List[SENSOR_PROPERTY_USE_GYRO].Value)); + } + + // + // Accelerometer related data field properties + // + { + Size = SENSOR_COLLECTION_LIST_SIZE(SENSOR_ACC_DATA_FIELD_PROPERTY_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pAccDataFieldProperties); + if (!NT_SUCCESS(Status) || m_pAccDataFieldProperties == nullptr) + { + TraceError("FUS %!FUNC! Fusion sensor WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pAccDataFieldProperties, Size); + m_pAccDataFieldProperties->Count = SENSOR_ACC_DATA_FIELD_PROPERTY_COUNT; + + m_pAccDataFieldProperties->List[SENSOR_ACC_RESOLUTION].Key = PKEY_SensorDataField_Resolution; + InitPropVariantFromFloat((float)AccFakeDevice_Axis_Resolution, + &(m_pAccDataFieldProperties->List[SENSOR_ACC_RESOLUTION].Value)); + + m_pAccDataFieldProperties->List[SENSOR_ACC_MIN_RANGE].Key = PKEY_SensorDataField_RangeMinimum; + InitPropVariantFromFloat(AccFakeDevice_Axis_Minimum, + &(m_pAccDataFieldProperties->List[SENSOR_ACC_MIN_RANGE].Value)); + + m_pAccDataFieldProperties->List[SENSOR_ACC_MAX_RANGE].Key = PKEY_SensorDataField_RangeMaximum; + InitPropVariantFromFloat(AccFakeDevice_Axis_Maximum, + &(m_pAccDataFieldProperties->List[SENSOR_ACC_MAX_RANGE].Value)); + + } + + // + // Gyroscope related data field properties + // + { + Size = SENSOR_COLLECTION_LIST_SIZE(SENSOR_GYR_DATA_FIELD_PROPERTY_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pGyrDataFieldProperties); + if (!NT_SUCCESS(Status) || m_pGyrDataFieldProperties == nullptr) + { + TraceError("FUS %!FUNC! Fusion sensor WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pGyrDataFieldProperties, Size); + m_pGyrDataFieldProperties->Count = SENSOR_GYR_DATA_FIELD_PROPERTY_COUNT; + + m_pGyrDataFieldProperties->List[SENSOR_GYR_RESOLUTION].Key = PKEY_SensorDataField_Resolution; + InitPropVariantFromFloat(GyrFakeDevice_Resolution_DegreesPerSecond, + &(m_pGyrDataFieldProperties->List[SENSOR_GYR_RESOLUTION].Value)); + + m_pGyrDataFieldProperties->List[SENSOR_GYR_MIN_RANGE].Key = PKEY_SensorDataField_RangeMinimum; + InitPropVariantFromFloat(GyrFakeDevice_Minimum_DegreesPerSecond, + &(m_pGyrDataFieldProperties->List[SENSOR_GYR_MIN_RANGE].Value)); + + m_pGyrDataFieldProperties->List[SENSOR_GYR_MAX_RANGE].Key = PKEY_SensorDataField_RangeMaximum; + InitPropVariantFromFloat(GyrFakeDevice_Maximum_DegreesPerSecond, + &(m_pGyrDataFieldProperties->List[SENSOR_GYR_MAX_RANGE].Value)); + } + + // + // Set default threshold + // + { + Size = SENSOR_COLLECTION_LIST_SIZE(FUSIONSENSOR_THRESHOLD_COUNT); + + MemoryHandle = NULL; + WDF_OBJECT_ATTRIBUTES_INIT(&MemoryAttributes); + MemoryAttributes.ParentObject = SensorInstance; + Status = WdfMemoryCreate(&MemoryAttributes, + PagedPool, + SENSOR_POOL_TAG_FUSIONSENSOR, + Size, + &MemoryHandle, + (PVOID*)&m_pThresholds); + if (!NT_SUCCESS(Status) || m_pThresholds == nullptr) + { + TraceError("FUS %!FUNC! WdfMemoryCreate failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_COLLECTION_LIST_INIT(m_pThresholds, Size); + m_pThresholds->Count = FUSIONSENSOR_THRESHOLD_COUNT; + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_ANGLE].Key = PKEY_SensorData_RotationAngle_Degrees; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_ANGLE].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_X].Key = PKEY_SensorData_LinearAccelerationX_Gs; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_X].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Y].Key = PKEY_SensorData_LinearAccelerationY_Gs; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Y].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Z].Key = PKEY_SensorData_LinearAccelerationZ_Gs; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_LINEAR_ACCELERATION_Z].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_X].Key = PKEY_SensorData_CorrectedAngularVelocityX_DegreesPerSecond; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_X].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Y].Key = PKEY_SensorData_CorrectedAngularVelocityY_DegreesPerSecond; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Y].Value)); + + m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Z].Key = PKEY_SensorData_CorrectedAngularVelocityZ_DegreesPerSecond; + InitPropVariantFromFloat(0.0, &(m_pThresholds->List[FUSIONSENSOR_THRESHOLD_ROTATION_RATE_Z].Value)); + + ZeroMemory(&m_CachedThresholds, sizeof(m_CachedThresholds)); + } + + ZeroMemory(&m_LastSample, sizeof(m_LastSample)); + + // Set default threshold + m_FirstSample = TRUE; + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// This routine is the AddDevice entry point for the FusionSensor client +// driver. This routine is called by the framework in response to AddDevice +// call from the PnP manager. It will create and initialize the device object +// to represent a new instance of the sensor client. +NTSTATUS +FusionSensorDevice::OnDeviceAdd( + _In_ WDFDRIVER /*Driver*/, // Supplies a handle to the driver object created in DriverEntry + _Inout_ PWDFDEVICE_INIT pDeviceInit // Supplies a pointer to a framework-allocated WDFDEVICE_INIT structure + ) +{ + WDF_PNPPOWER_EVENT_CALLBACKS Callbacks; + WDFDEVICE Device = nullptr; + WDF_OBJECT_ATTRIBUTES FdoAttributes; + ULONG Flag = 0; + SENSOR_CONTROLLER_CONFIG SensorConfig; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + WDF_OBJECT_ATTRIBUTES_INIT(&FdoAttributes); + + // Initialize FDO attributes and set up file object with sensor extension + Status = SensorsCxDeviceInitConfig(pDeviceInit, &FdoAttributes, Flag); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! SensorsCxDeviceInitConfig failed %!STATUS!", Status); + goto Exit; + } + + // Register the PnP callbacks with the framework. + WDF_PNPPOWER_EVENT_CALLBACKS_INIT(&Callbacks); + Callbacks.EvtDevicePrepareHardware = FusionSensorDevice::OnPrepareHardware; + Callbacks.EvtDeviceReleaseHardware = FusionSensorDevice::OnReleaseHardware; + Callbacks.EvtDeviceD0Entry = FusionSensorDevice::OnD0Entry; + Callbacks.EvtDeviceD0Exit = FusionSensorDevice::OnD0Exit; + + WdfDeviceInitSetPnpPowerEventCallbacks(pDeviceInit, &Callbacks); + + // Call the framework to create the device + Status = WdfDeviceCreate(&pDeviceInit, &FdoAttributes, &Device); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! WdfDeviceCreate failed %!STATUS!", Status); + goto Exit; + } + + // Register CLX callback function pointers + SENSOR_CONTROLLER_CONFIG_INIT(&SensorConfig); + SensorConfig.DriverIsPowerPolicyOwner = WdfUseDefault; + + SensorConfig.EvtSensorStart = FusionSensorDevice::OnStart; + SensorConfig.EvtSensorStop = FusionSensorDevice::OnStop; + SensorConfig.EvtSensorGetSupportedDataFields = FusionSensorDevice::OnGetSupportedDataFields; + SensorConfig.EvtSensorGetDataInterval = FusionSensorDevice::OnGetDataInterval; + SensorConfig.EvtSensorSetDataInterval = FusionSensorDevice::OnSetDataInterval; + SensorConfig.EvtSensorGetDataFieldProperties = FusionSensorDevice::OnGetDataFieldProperties; + SensorConfig.EvtSensorGetDataThresholds = FusionSensorDevice::OnGetDataThresholds; + SensorConfig.EvtSensorSetDataThresholds = FusionSensorDevice::OnSetDataThresholds; + SensorConfig.EvtSensorGetProperties = FusionSensorDevice::OnGetProperties; + SensorConfig.EvtSensorDeviceIoControl = FusionSensorDevice::OnIoControl; + + // Set up power capabilities and IO queues + Status = SensorsCxDeviceInitialize(Device, &SensorConfig); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! SensorsCxDeviceInitialize failed %!STATUS!", Status); + goto Exit; + } + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// This routine is called by the framework when the PnP manager sends an +// IRP_MN_START_DEVICE request to the driver stack. This routine is +// responsible for performing operations that are necessary to make the +// driver's device operational (for e.g. mapping the hardware resources +// into memory). +NTSTATUS +FusionSensorDevice::OnPrepareHardware( + _In_ WDFDEVICE Device, // Supplies a handle to the framework device object + _In_ WDFCMRESLIST /*ResourcesRaw*/, // Supplies a handle to a collection of framework resource + // objects. This collection identifies the raw (bus-relative) hardware + // resources that have been assigned to the device. + _In_ WDFCMRESLIST /*ResourcesTranslated*/) // Supplies a handle to a collection of framework + // resource objects. This collection identifies the translated + // (system-physical) hardware resources that have been assigned to the + // device. The resources appear from the CPU's point of view. +{ + PFusionSensorDevice pDevice = nullptr; + WDF_OBJECT_ATTRIBUTES SensorAttr = {}; + SENSOR_CONFIG SensorConfig = {}; + SENSOROBJECT SensorInstance = nullptr; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + // Construct sensor instance + + // Create WDFOBJECT for the sensor + WDF_OBJECT_ATTRIBUTES_INIT_CONTEXT_TYPE(&SensorAttr, FusionSensorDevice); + + // Register sensor instance with clx + + Status = SensorsCxSensorCreate(Device, &SensorAttr, &SensorInstance); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! SensorsCxSensorCreate failed %!STATUS!", Status); + goto Exit; + } + + pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + if (nullptr == pDevice) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetFusionSensorContextFromSensorInstance failed %!STATUS!", Status); + goto Exit; + } + + // Device initialization + + Status = pDevice->Initialize(Device, SensorInstance); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! Initialize device object failed %!STATUS!", Status); + goto Exit; + } + + SENSOR_CONFIG_INIT(&SensorConfig); + SensorConfig.pEnumerationList = pDevice->m_pEnumerationProperties; + Status = SensorsCxSensorInitialize(SensorInstance, &SensorConfig); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! SensorsCxSensorInitialize failed %!STATUS!", Status); + goto Exit; + } + +Exit: + SENSOR_FunctionExit(Status); + + return Status; +} + + + +// This routine is called by the framework when the PnP manager is revoking +// ownership of our resources. This may be in response to either +// IRP_MN_STOP_DEVICE or IRP_MN_REMOVE_DEVICE. This routine is responsible for +// performing cleanup of resources allocated in PrepareHardware callback. +// This callback is invoked before passing the request down to the lower driver. +// This routine will also be invoked by the framework if the prepare hardware +// callback returns a failure. +// +// Argument: +// Device: IN: Supplies a handle to the framework device object +// ResourcesTranslated: IN: Supplies a handle to a collection of framework +// resource objects. This collection identifies the translated +// (system-physical) hardware resources that have been assigned to the +// device. The resources appear from the CPU's point of view. +// +// Return Value: +// NTSTATUS code +//------------------------------------------------------------------------------ +NTSTATUS +FusionSensorDevice::OnReleaseHardware( + _In_ WDFDEVICE Device, // Supplies a handle to the framework device object + _In_ WDFCMRESLIST /*ResourcesTranslated*/) // Supplies a handle to a collection of framework + // resource objects. This collection identifies the translated + // (system-physical) hardware resources that have been assigned to the + // device. The resources appear from the CPU's point of view. +{ + PHardwareSimulator pSimulator = nullptr; + PFusionSensorDevice pDevice = nullptr; + SENSOROBJECT SensorInstance = nullptr; + ULONG SensorInstanceCount = 1; // only expect 1 sensor instance + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + // Get sensor instance + Status = SensorsCxDeviceGetSensorList(Device, &SensorInstance, &SensorInstanceCount); + if (!NT_SUCCESS(Status) || + 0 == SensorInstanceCount || + NULL == SensorInstance) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! SensorsCxDeviceGetSensorList failed %!STATUS!", Status); + goto Exit; + } + + pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! GetFusionSensorContextFromSensorInstance failed %!STATUS!", Status); + goto Exit; + } + + // TODO: Remove the HardwareSimulator code in your final driver. The HardwareSimulator is only used for the purpose of demonstrating how sensor driver samples work. + pSimulator = GetHardwareSimulatorContextFromInstance(pDevice->m_SimulatorInstance); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + goto Exit; + } + + + // Delete lock + if (NULL != pDevice->m_Lock) + { + WdfObjectDelete(pDevice->m_Lock); + pDevice->m_Lock = NULL; + } + + // Cleanup the FusionSensor simulator + pSimulator->Cleanup(); + + // Delete hardware simulator instance + if (NULL != pDevice->m_SimulatorInstance) + { + WdfObjectDelete(pDevice->m_SimulatorInstance); + pDevice->m_SimulatorInstance = NULL; + } + + // Delete sensor instance + if (NULL != pDevice->m_SensorInstance) + { + WdfObjectDelete(pDevice->m_SensorInstance); + } + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// This routine is invoked by the framework to program the device to goto +// D0, which is the working state. The framework invokes callback every +// time the hardware needs to be (re-)initialized. This includes after +// IRP_MN_START_DEVICE, IRP_MN_CANCEL_STOP_DEVICE, IRP_MN_CANCEL_REMOVE_DEVICE, +// and IRP_MN_SET_POWER-D0. +NTSTATUS +FusionSensorDevice::OnD0Entry( + _In_ WDFDEVICE Device, // Supplies a handle to the framework device object + _In_ WDF_POWER_DEVICE_STATE /*PreviousState*/) // WDF_POWER_DEVICE_STATE-typed enumerator that identifies + // the device power state that the device was in before this transition to D0 +{ + PFusionSensorDevice pDevice; + SENSOROBJECT SensorInstance = NULL; + ULONG SensorInstanceCount = 1; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + // Get sensor instance + Status = SensorsCxDeviceGetSensorList(Device, &SensorInstance, &SensorInstanceCount); + if (!NT_SUCCESS(Status) || + 0 == SensorInstanceCount || + NULL == SensorInstance) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! SensorsCxDeviceGetSensorList failed %!STATUS!", Status); + goto Exit; + } + + pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! GetFusionSensorContextFromSensorInstance failed %!STATUS!", Status); + goto Exit; + } + + // + // Power on sensor + // + pDevice->m_PoweredOn = TRUE; + InitPropVariantFromUInt32(SensorState_Idle, + &(pDevice->m_pProperties->List[SENSOR_PROPERTY_STATE].Value)); + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} + + + +// This routine is invoked by the framework to program the device to go into +// a certain Dx state. The framework invokes callback every the the device is +// leaving the D0 state, which happens when the device is stopped, when it is +// removed, and when it is powered off. +NTSTATUS +FusionSensorDevice::OnD0Exit( + _In_ WDFDEVICE Device, // Supplies a handle to the framework device object + _In_ WDF_POWER_DEVICE_STATE /*TargetState*/) // Supplies the device power state which the device will be put + // in once the callback is complete +{ + PFusionSensorDevice pDevice; + SENSOROBJECT SensorInstance = NULL; + ULONG SensorInstanceCount = 1; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + // Get sensor instance + Status = SensorsCxDeviceGetSensorList(Device, &SensorInstance, &SensorInstanceCount); + if (!NT_SUCCESS(Status) || + 0 == SensorInstanceCount || + NULL == SensorInstance) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! SensorsCxDeviceGetSensorList failed %!STATUS!", Status); + goto Exit; + } + + pDevice = GetFusionSensorContextFromSensorInstance(SensorInstance); + if (nullptr == pDevice) + { + Status = STATUS_INVALID_PARAMETER; + TraceError("FUS %!FUNC! GetFusionSensorContextFromSensorInstance failed %!STATUS!", Status); + goto Exit; + } + + // + // Power on sensor + // + pDevice->m_PoweredOn = FALSE; + +Exit: + SENSOR_FunctionExit(Status); + return Status; +} diff --git a/sensors/Fusion/driver.cpp b/sensors/Fusion/driver.cpp new file mode 100644 index 00000000..1fd117f3 --- /dev/null +++ b/sensors/Fusion/driver.cpp @@ -0,0 +1,76 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved. +// +//Abstract: +// +// This module contains the implementation of entry and exit point of FusionSensor sample driver. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#include "Device.h" +#include "Driver.h" + +#include "Driver.tmh" + + + +// This routine is the driver initialization entry point. +NTSTATUS +DriverEntry( + _In_ PDRIVER_OBJECT DriverObject, // Pointer to the driver object created by the I/O manager + _In_ PUNICODE_STRING RegistryPath // Pointer to the driver specific registry key + ) +{ + WDF_DRIVER_CONFIG DriverConfig; + NTSTATUS Status = STATUS_SUCCESS; + + // Initialize WPP Tracing + WPP_INIT_TRACING(DriverObject, NULL); + + SENSOR_FunctionEnter(); + + DriverConfig.DriverPoolTag = SENSOR_POOL_TAG_FUSIONSENSOR; + + // Initialize the driver configuration structure. + WDF_DRIVER_CONFIG_INIT(&DriverConfig, FusionSensorDevice::OnDeviceAdd); + DriverConfig.EvtDriverUnload = OnDriverUnload; + + // Create a framework driver object to represent our driver. + Status = WdfDriverCreate(DriverObject, + RegistryPath, + WDF_NO_OBJECT_ATTRIBUTES, + &DriverConfig, + WDF_NO_HANDLE); + + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! WdfDriverCreate failed: %!STATUS!", Status); + goto Exit; + } + +Exit: + SENSOR_FunctionExit(Status); + + return Status; +} + + + +// This routine is called when the driver unloads. +VOID +OnDriverUnload( + _In_ WDFDRIVER Driver // Driver object + ) +{ + SENSOR_FunctionEnter(); + + SENSOR_FunctionExit(STATUS_SUCCESS); + + // WPP_CLEANUP doesn't actually use the Driver parameter + // So we need to set it as unreferenced. + UNREFERENCED_PARAMETER(Driver); + WPP_CLEANUP(WdfDriverWdmGetDriverObject(Driver)); + + return; +}
\ No newline at end of file diff --git a/sensors/Fusion/hardwaresimulator.cpp b/sensors/Fusion/hardwaresimulator.cpp new file mode 100644 index 00000000..b498defb --- /dev/null +++ b/sensors/Fusion/hardwaresimulator.cpp @@ -0,0 +1,277 @@ +//Copyright (C) Microsoft Corporation, All Rights Reserved. +// +//Abstract: +// +// This module contains the implementation of the FusionSensor sample driver +// hardware simulator. +// +//Environment: +// +// Windows User-Mode Driver Framework (UMDF) + +#include "HardwareSimulator.h" + +#include "HardwareSimulator.tmh" + +#include "Device.h" + +// Simulated FusionSensor data +// The simulation data represent the FusionSensor data for a user walking and running at different paces +// Since the simulator is designed to report a sample every second, +// each line in the table represents 1 second of data +const FusionSensorSample SimulatorData[] = { +// 1: Timestamp +// 2: Quaternion; +// 3: Accuracy; +// 4: DeclinationAngle; +// +// 1 | 2 | 3 | 4 | + { {},{ 0.6118738f, 0.1390667f, 0.136009f, 0.7658455f }, MagnetometerAccuracy_Unknown, 17.1f }, + { {},{ 0.6174188f, 0.1399692f, 0.1338535f, 0.7625271f }, MagnetometerAccuracy_Approximate, 17.1f }, + { {},{ 0.6028171f, 0.1740682f, 0.1788765f, 0.7570137f }, MagnetometerAccuracy_High, 17.1f }, + { {},{ 0.4333673f, 0.05344541f, 0.06945555f, 0.8965058f }, MagnetometerAccuracy_High, 17.1f }, +}; + +HardwareSimulator::HardwareSimulator() : + m_HasReset(TRUE), + m_Index(0), + m_Lock(NULL), + m_State(SimulatorState_NotInitialized), + m_SimulatorInstance(NULL), + m_Timer(NULL) +{ +} + +HardwareSimulator::~HardwareSimulator() +{ +} + +// This static routine performs simulator initialization. The routine creates a +// timer object that periodically updates the m_Index location +NTSTATUS +HardwareSimulator::Initialize( + _In_ WDFDEVICE Device, // WDF device representing the sensor + _Out_ WDFOBJECT *SimulatorInstance) // Instance of the WDF object for the simulator +{ + PHardwareSimulator pSimulator = nullptr; + NTSTATUS Status = STATUS_SUCCESS; + WDF_OBJECT_ATTRIBUTES HardwareSimulatorAttributes = {}; + + SENSOR_FunctionEnter(); + + // Create WDFOBJECT for the hardware simulator + WDF_OBJECT_ATTRIBUTES_INIT_CONTEXT_TYPE(&HardwareSimulatorAttributes, HardwareSimulator); + HardwareSimulatorAttributes.ParentObject = Device; + + Status = WdfObjectCreate(&HardwareSimulatorAttributes, SimulatorInstance); + if (!NT_SUCCESS(Status)) + { + TraceError("FUS %!FUNC! WdfObjectCreate failed %!STATUS!", Status); + goto Exit; + } + + pSimulator = GetHardwareSimulatorContextFromInstance(*SimulatorInstance); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + goto Exit; + } + + pSimulator->InitializeInternal(*SimulatorInstance); + +Exit: + + SENSOR_FunctionExit(Status); + + return Status; +} + + +// Internal routine to perform simulator initialization +NTSTATUS +HardwareSimulator::InitializeInternal( + _In_ WDFOBJECT SimulatorInstance) // Instance of the WDF object for the simulator +{ + NTSTATUS Status = STATUS_SUCCESS; + WDF_OBJECT_ATTRIBUTES TimerAttributes = {}; + WDF_TIMER_CONFIG TimerConfig = {}; + + SENSOR_FunctionEnter(); + + // Only initialize the simulator if it is in the "not initialized" state + if (SimulatorState_NotInitialized == m_State) + { + // Create sample Lock + Status = WdfWaitLockCreate(WDF_NO_OBJECT_ATTRIBUTES, &m_Lock); + if (!NT_SUCCESS(Status)) + { + m_Lock = NULL; + + TraceError("FUS %!FUNC! WdfWaitLockCreate for m_Lock failed %!STATUS!", Status); + goto Exit; + } + + // Create a timer object for simulation updates + WDF_TIMER_CONFIG_INIT(&TimerConfig, HardwareSimulator::OnTimerExpire); + WDF_OBJECT_ATTRIBUTES_INIT(&TimerAttributes); + TimerAttributes.ParentObject = SimulatorInstance; + TimerAttributes.ExecutionLevel = WdfExecutionLevelPassive; + + Status = WdfTimerCreate(&TimerConfig, &TimerAttributes, &m_Timer); + if (!NT_SUCCESS(Status)) + { + m_Timer = NULL; + + TraceError("FUS %!FUNC! WdfTimerCreate failed %!STATUS!", Status); + goto Exit; + } + + // Set the simulator state to "initialized" + m_State = SimulatorState_Initialized; + m_SimulatorInstance = SimulatorInstance; + } + +Exit: + if (!NT_SUCCESS(Status) && NULL != m_Lock) + { + WdfObjectDelete(m_Lock); + m_Lock = NULL; + } + + SENSOR_FunctionExit(Status); + + return Status; +} + + +// This routine perform a simulator cleanup +NTSTATUS +HardwareSimulator::Cleanup() +{ + NTSTATUS status = STATUS_SUCCESS; + + if (SimulatorState_Started == m_State) + { + Stop(); + } + + // Delete lock + if (NULL != m_Lock) + { + WdfObjectDelete(m_Lock); + m_Lock = NULL; + } + + // Set the simulator state to "not initialized" + m_State = SimulatorState_NotInitialized; + + return status; +} + + +// This routine starts the simulator +NTSTATUS +HardwareSimulator::Start() +{ + NTSTATUS status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (SimulatorState_Initialized == m_State) + { + WdfTimerStart(m_Timer, WDF_REL_TIMEOUT_IN_MS(SIMULATOR_HARDWARE_INTERVAL_MS)); + m_State = SimulatorState_Started; + } + + SENSOR_FunctionExit(status); + + return status; +} + + +// This routine stops the simulator +NTSTATUS +HardwareSimulator::Stop() +{ + NTSTATUS status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (SimulatorState_Started == m_State) + { + WdfTimerStop(m_Timer, TRUE); + m_State = SimulatorState_Initialized; + } + + SENSOR_FunctionExit(status); + + return status; +} + + +// This callback is called when the simulator wait time has expired and the simulator +// is ready to switch to the next sample. The callback updates the sample index and +// schedules the next wake up time. +VOID +HardwareSimulator::OnTimerExpire( + _In_ WDFTIMER Timer) // WDF timer object +{ + HardwareSimulator *pSimulator = nullptr; + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + pSimulator = GetHardwareSimulatorContextFromInstance(WdfTimerGetParentObject(Timer)); + if (nullptr == pSimulator) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! GetHardwareSimulatorContextFromInstance failed %!STATUS!", Status); + } + + if (NT_SUCCESS(Status)) + { + // Increment the sample index, roll over if the index reach the end of the array + WdfWaitLockAcquire(pSimulator->m_Lock, NULL); + pSimulator->m_Index++; + pSimulator->m_Index = pSimulator->m_Index % ARRAYSIZE(SimulatorData); + + WdfWaitLockRelease(pSimulator->m_Lock); + + WdfTimerStart(pSimulator->m_Timer, WDF_REL_TIMEOUT_IN_MS(SIMULATOR_HARDWARE_INTERVAL_MS)); + } + + SENSOR_FunctionExit(Status); +} + + +// This routine returns the current sample from the driver at the current m_Index +// location +NTSTATUS +HardwareSimulator::GetSample( + _Out_ FusionSensorSample *Sample) // FusionSensor sample +{ + NTSTATUS Status = STATUS_SUCCESS; + + SENSOR_FunctionEnter(); + + if (nullptr == Sample) + { + Status = STATUS_INSUFFICIENT_RESOURCES; + TraceError("FUS %!FUNC! Sample parameter is null"); + } + + if (NT_SUCCESS(Status)) + { + WdfWaitLockAcquire(m_Lock, NULL); + *Sample = SimulatorData[m_Index]; + + WdfWaitLockRelease(m_Lock); + + GetSystemTimePreciseAsFileTime(&Sample->Timestamp); + } + + SENSOR_FunctionExit(Status); + + return Status; +} diff --git a/sensors/Fusion/readme.md b/sensors/Fusion/readme.md new file mode 100644 index 00000000..3a4bb2bf --- /dev/null +++ b/sensors/Fusion/readme.md @@ -0,0 +1,3 @@ +### Fusion Sensor Driver Sample + +The FusionSensor sample shows how to write a UMDF v2 driver to control a virtual FusionSensor sensor.
\ No newline at end of file |
