DWORD WINAPI SensorFusionProc(LPVOID lpParam) { #define MAGNET_VELOCITY 1 float dT = 0.05f, fs = 1.0f/dT, mfs = fs; float ax, ay, az; float mx, my, mz; float gx, gy, gz; #ifndef MAGNET_VELOCITY Rotation *rotation = NULL; ISTBOOL rotation_done = ISTFALSE; #endif Magnet *magnet = NULL; ISTBOOL magnet_done = ISTFALSE; ISTFLOAT ws[3] = {0}; ISTFLOAT gs[3] = {0}; ISTFLOAT gdata[3]; ISTFLOAT mdata[3]; CSensorBase *accel_sensor = NULL; CSensorBase *magnet_sensor = NULL; CSensorBase *gyro_sensor = NULL; AccelerationCalibration *accel_cal = NULL; AcceleratorMeasurement cx, cy, cz; float g; float tmA, tmM; DWORD dwRet, dwCnt, dwStart, dwEnd, dwTimeout; DWORD samples = (DWORD)(fs/mfs); DWORD period = (DWORD)(1000.0f/fs); HANDLE event = NULL; IST_DBG("+++ Enter SensorFusionProc() +++\n"); FreezeGUI(false); accel_sensor = CreateSensor(DEV_ID_KIONIX, fs, g_pMain->m_iSenI2c); if (!accel_sensor) { IST_DBG("!accel_sensor\n"); goto EXIT; } magnet_sensor = CreateSensor(DEV_ID_ISENTEK_8303, mfs, g_pMain->m_iSenI2c); if (!magnet_sensor) { IST_DBG("!magnet_sensor\n"); goto EXIT; } gyro_sensor = CreateSensor(DEV_ID_LG3GYRO, fs, g_pMain->m_iSenI2c); if (!gyro_sensor) { IST_DBG("!gyro_sensor\n"); goto EXIT; } magnet = IST_Magnet()->New(); if (!magnet) { IST_DBG("!magnet\n"); goto EXIT; } #ifndef MAGNET_VELOCITY rotation = IST_Rotation()->New(); if (!rotation) { IST_DBG("!rotation\n"); goto EXIT; } #endif #define GRAVITY_SENSOR_POSITIVE_X_OFFSET 12.600f #define GRAVITY_SENSOR_NEGATIVE_X_OFFSET -8.450f #define GRAVITY_SENSOR_ZERO_X_OFFSET 1.838f #define GRAVITY_SENSOR_POSITIVE_Y_OFFSET 11.350f #define GRAVITY_SENSOR_NEGATIVE_Y_OFFSET -9.365f #define GRAVITY_SENSOR_ZERO_Y_OFFSET 0.981f #define GRAVITY_SENSOR_POSITIVE_Z_OFFSET 14.920f #define GRAVITY_SENSOR_NEGATIVE_Z_OFFSET -7.000f #define GRAVITY_SENSOR_ZERO_Z_OFFSET 3.154f #define GRAVITY_SENSOR_G 9.81f g = GRAVITY_SENSOR_G; cx.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_X_OFFSET; cx.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_X_OFFSET; cx.at_zero_gravity = GRAVITY_SENSOR_ZERO_X_OFFSET; cy.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_Y_OFFSET; cy.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_Y_OFFSET; cy.at_zero_gravity = GRAVITY_SENSOR_ZERO_Y_OFFSET; cz.at_positive_gravity = GRAVITY_SENSOR_POSITIVE_Z_OFFSET; cz.at_negative_gravity = GRAVITY_SENSOR_NEGATIVE_Z_OFFSET; cz.at_zero_gravity = GRAVITY_SENSOR_ZERO_Z_OFFSET; accel_cal = CAccelerationCalibration()->New(g, cx, cy, cz); if (!accel_cal) { IST_DBG("!accel_cal\n"); goto EXIT; } event = CreateEvent(NULL, TRUE, FALSE, NULL); if (!event) { goto EXIT; } dwCnt = 0; tmM = tmA = GetCurrentSeconds(); dwTimeout = period; while (!g_pMain->bStopSignal) { xIST_DBG("dwTimeout = %u ms\n", dwTimeout); dwRet = WaitForSingleObject(event, dwTimeout); if (WAIT_TIMEOUT != dwRet) { xIST_DBG("dwRet = %u\n", dwRet); g_pMain->bStopSignal = true; continue; } dwStart = GetTickCount(); /* get real gyroscope data */ if (FALSE == ReadSensorData(gyro_sensor, &gx, &gy, &gz, CSensorBase::GYRO_UNITS::CONFIG_GYRO_UNITS_RAD)){ g_pMain->bStopSignal = true; continue; } gs[0] = FloatType(gx); gs[1] = FloatType(gy); gs[2] = FloatType(gz); /* Get accelerator data */ if (FALSE == ReadSensorData(accel_sensor, &ax, &ay, &az, CSensorBase::ACCEL_UNITS::CONFIG_ACCEL_UNITS_MPSS)){ g_pMain->bStopSignal = true; continue; } if (accel_cal->Process(accel_cal, ax, ay, az, dT)) { accel_cal->GetData(accel_cal, &ax, &ay, &az); } gdata[0] = FloatType(ax); gdata[1] = FloatType(ay); gdata[2] = FloatType(az); xIST_DBG("a[%14.10f,%14.10f,%14.10f]\n", ax, ay, az); /* Get magnet field data */ if (FALSE == ReadSensorData(magnet_sensor, &mx, &my, &mz, CSensorBase::MAG_UNITS::CONFIG_MAG_UNITS_UT)){ g_pMain->bStopSignal = true; continue; } dT = EllapsedSeconds(&tmM); mdata[0] = FloatType(mx); mdata[1] = FloatType(my); mdata[2] = FloatType(mz); xIST_DBG("m[%14.10f,%14.10f,%14.10f]\n", mx, my, mz); #ifndef MAGNET_VELOCITY rotation_done = rotation->Process(rotation, mdata, gdata, FloatType(dT)); #endif magnet_done = magnet->Process(magnet, mdata, FloatType(dT)); #ifndef MAGNET_VELOCITY /* check valid */ xIST_DBG("rotation_done:%s\n", rotation_done ? "YES" : "NO"); #endif xIST_DBG("magnet_done:%s\n", magnet_done ? "YES" : "NO"); #ifndef MAGNET_VELOCITY if (rotation_done && magnet_done) { #else if (magnet_done) { #endif CString tmpStr; if (g_pMain->GetCurrentPage() == CiSensorsDlg::GYROSCOPE_DATA) { xIST_DBG("update gyro\n"); #ifdef MAGNET_VELOCITY ws[0] = magnet->Velocity[0]; ws[1] = magnet->Velocity[1]; ws[2] = magnet->Velocity[2]; #else ws[0] = rotation->Velocity[0]; ws[1] = rotation->Velocity[1]; ws[2] = rotation->Velocity[2]; #endif xIST_DBG("%14.10f,%14.10f,%14.10f,%14.10f,%14.10f,%14.10f\n", gx, gy, gz, (float)NativeType(ws[0]), (float)NativeType(ws[1]), (float)NativeType(ws[2])); for (int i = 0; i < 3; ++i) { tmpStr.Format(_T("%0.2f"), (float)NativeType(ws[i])); g_pMain->m_SubDlgGyroscopeData.GetDlgItem(IDC_STATIC_EGYROSCOPE_X + i)->SetWindowTextW(tmpStr); tmpStr.Format(_T("%0.2f"), (float)NativeType(gs[i])); g_pMain->m_SubDlgGyroscopeData.GetDlgItem(IDC_STATIC_GYROSCOPE_X + i)->SetWindowTextW(tmpStr); g_pMain->m_SubDlgGyroscopeData.m_GyroWaveform[i].SetData( (float)NativeType(gs[0]), (float)NativeType(gs[1]), (float)NativeType(gs[2])); g_pMain->m_SubDlgGyroscopeData.m_SoftGyroWaveform[i].SetData( (float)NativeType(ws[0]), (float)NativeType(ws[1]), (float)NativeType(ws[2])); } } } dwEnd = GetTickCount(); if (period > (dwEnd - dwStart)) dwTimeout = period - (dwEnd - dwStart); else dwTimeout = 0; } EXIT: if (event) { CloseHandle(event); event = NULL; } #ifndef MAGNET_VELOCITY if (rotation) { IST_Rotation()->Delete(rotation); rotation = NULL; } #endif if (magnet) { IST_Magnet()->Delete(magnet); magnet = NULL; } DestroySensor(&gyro_sensor); DestroySensor(&magnet_sensor); DestroySensor(&accel_sensor); FreezeGUI(true); IST_DBG("--- Leave SensorFusionProc() ---\n"); ExitThread(0); }
NTSTATUS CCaptureDevice:: PnpStart ( _In_ PCM_RESOURCE_LIST TranslatedResourceList, _In_ PCM_RESOURCE_LIST UntranslatedResourceList ) /*++ Routine Description: Called at Pnp start. We start up our hardware simulation. Arguments: TranslatedResourceList - The translated resource list from Pnp UntranslatedResourceList - The untranslated resource list from Pnp Return Value: Success / Failure --*/ { PAGED_CODE(); // // Normally, we'd do things here like parsing the resource lists and // connecting our interrupt. Since this is a simulation, there isn't // much to parse. The parsing and connection should be the same as // any WDM driver. The sections that will differ are illustrated below // in setting up a simulated DMA. // NTSTATUS Status = STATUS_SUCCESS; // // By PnP, it's possible to receive multiple starts without an intervening // stop (to reevaluate resources, for example). Thus, we only perform // creations of the simulation on the initial start and ignore any // subsequent start. Hardware drivers with resources should evaluate // resources and make changes on 2nd start. // // Walk the list of descriptors and enumerate the h/w sims described. // if (!m_Device -> Started) { // Create the Filter for the device for( size_t i=0; i<m_FilterDescriptorCount; i++ ) { PKSFILTERFACTORY FilterFactory=nullptr; IFFAILED_EXIT( KsCreateFilterFactory( m_Device->FunctionalDeviceObject, m_Context[i].Descriptor, m_Context[i].Name, NULL, KSCREATE_ITEM_FREEONSTOP, NULL, NULL, &FilterFactory ) ); // Set primary camera to its interface, this allows device to be queried by GUID if( FilterFactory ) { PKSDEVICE_PROFILE_INFO pDeviceProfiles = nullptr; UINT32 uiProfileCount = 0; PUNICODE_STRING SymbolicLinkName = KsFilterFactoryGetSymbolicLink(FilterFactory); BOOL fFrontCamera = FALSE; ACPI_PLD_V2_BUFFER pld = {0}; pld.Revision = 2; pld.Panel = m_Context[i].AcpiPosition; pld.Rotation = m_Context[i].AcpiRotation; static const GUID FFC_Filter = {STATIC_FrontCamera_Filter}; IFFAILED_EXIT( IoSetDeviceInterfacePropertyData(SymbolicLinkName, &DEVPKEY_Device_PhysicalDeviceLocation, LOCALE_NEUTRAL, PLUGPLAY_PROPERTY_PERSISTENT, DEVPROP_TYPE_BINARY, sizeof(pld), (PVOID)&pld) ); if( m_Context[i].Descriptor->ReferenceGuid && IsEqualGUID(*(m_Context[i].Descriptor->ReferenceGuid), FFC_Filter)) { fFrontCamera = TRUE; } // Publish our profile here. uiProfileCount = InitializeDeviceProfiles(fFrontCamera, &pDeviceProfiles); if( uiProfileCount > 0 && pDeviceProfiles != nullptr) { if( NT_SUCCESS(KsInitializeDeviceProfile(FilterFactory)) ) { for( UINT32 j=0; j<uiProfileCount; j++ ) { if( !NT_SUCCESS(KsPublishDeviceProfile(FilterFactory, &pDeviceProfiles[j])) ) { // Bail... break; } } if( !NT_SUCCESS(KsPersistDeviceProfile(FilterFactory)) ) { // Trace here? } } ExFreePool(pDeviceProfiles); pDeviceProfiles = NULL; } else { IFFAILED_EXIT(STATUS_INSUFFICIENT_RESOURCES); } } // On a real device this object would be constructed whenever the sensor hardware is ready... m_Sensor[i] = CreateSensor( m_Context[i].Descriptor ); if( !m_Sensor[i] ) { Status = STATUS_INSUFFICIENT_RESOURCES; break; } IFFAILED_EXIT(m_Sensor[i]->Initialize()); m_Sensor[i]->SetMountingOrientation( m_Context[i].AcpiRotation ); } } done: if( !NT_SUCCESS(Status) ) { // Free any sensor objects we created during this failed attempt. for( size_t i=0; i<m_FilterDescriptorCount; i++ ) { SAFE_DELETE( m_Sensor[i] ); } } return Status; }