void apply_state( const Eigen::Quaternionf &new_orientation, const Eigen::Vector3f &new_angular_velocity, const Eigen::Vector3f &new_angular_acceleration) { if (eigen_quaternion_is_valid(new_orientation)) { orientation = new_orientation; } else { SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!"; } if (eigen_vector3f_is_valid(new_angular_velocity)) { angular_velocity= new_angular_velocity; } else { SERVER_LOG_WARNING("OrientationFilter") << "Angular Velocity is NaN!"; } if (eigen_vector3f_is_valid(new_angular_acceleration)) { angular_acceleration= new_angular_acceleration; } else { SERVER_LOG_WARNING("OrientationFilter") << "Angular Acceleration is NaN!"; } // state is valid now that we have had an update bIsValid= true; }
void OrientationFilter::update( const float delta_time, const OrientationSensorPacket &sensorPacket) { OrientationFilterPacket filterPacket; m_FilterSpace.convertSensorPacketToFilterPacket(sensorPacket, filterPacket); const Eigen::Quaternionf orientation_backup= m_FusionState->orientation; const Eigen::Vector3f first_derivative_backup = m_FusionState->angular_velocity; const Eigen::Vector3f second_derivative_backup = m_FusionState->angular_acceleration; switch(m_FusionState->fusion_type) { case FusionTypeNone: break; case FusionTypePassThru: { const Eigen::Quaternionf &new_orientation= filterPacket.orientation; const Eigen::Quaternionf orientation_derivative= Eigen::Quaternionf((new_orientation.coeffs() - m_FusionState->orientation.coeffs()) / delta_time); const Eigen::Vector3f angular_velocity = quaternion_derivative_to_angular_velocity(new_orientation, orientation_derivative); const Eigen::Vector3f angular_accelertion = (angular_velocity - m_FusionState->angular_velocity) / delta_time; m_FusionState->orientation = new_orientation; m_FusionState->angular_velocity = angular_velocity; m_FusionState->angular_acceleration = angular_accelertion; } break; case FusionTypeMadgwickIMU: orientation_fusion_imu_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; case FusionTypeMadgwickMARG: orientation_fusion_madgwick_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; case FusionTypeComplementaryMARG: orientation_fusion_complementary_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; } if (!eigen_quaternion_is_valid(m_FusionState->orientation)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!"; m_FusionState->orientation = orientation_backup; } if (!eigen_vector3f_is_valid(m_FusionState->angular_velocity)) { SERVER_LOG_WARNING("OrientationFilter") << "Angular Velocity is NaN!"; m_FusionState->angular_velocity = first_derivative_backup; } if (!eigen_vector3f_is_valid(m_FusionState->angular_acceleration)) { SERVER_LOG_WARNING("OrientationFilter") << "Angular Acceleration is NaN!"; m_FusionState->angular_acceleration = second_derivative_backup; } }
void MorpheusHMDConfig::ptree2config(const boost::property_tree::ptree &pt) { version = pt.get<int>("version", 0); if (version == MorpheusHMDConfig::CONFIG_VERSION) { is_valid = pt.get<bool>("is_valid", false); disable_command_interface= pt.get<bool>("disable_command_interface", disable_command_interface); prediction_time = pt.get<float>("prediction_time", 0.f); max_poll_failure_count = pt.get<long>("max_poll_failure_count", 100); // Use the current accelerometer values (constructor defaults) as the default values accelerometer_gain.i = pt.get<float>("Calibration.Accel.X.k", accelerometer_gain.i); accelerometer_gain.j = pt.get<float>("Calibration.Accel.Y.k", accelerometer_gain.j); accelerometer_gain.k = pt.get<float>("Calibration.Accel.Z.k", accelerometer_gain.k); raw_accelerometer_bias.i = pt.get<float>("Calibration.Accel.X.b", raw_accelerometer_bias.i); raw_accelerometer_bias.j = pt.get<float>("Calibration.Accel.Y.b", raw_accelerometer_bias.j); raw_accelerometer_bias.k = pt.get<float>("Calibration.Accel.Z.b", raw_accelerometer_bias.k); raw_accelerometer_variance = pt.get<float>("Calibration.Accel.Variance", raw_accelerometer_variance); // Use the current gyroscope values (constructor defaults) as the default values gyro_gain.i = pt.get<float>("Calibration.Gyro.X.k", gyro_gain.i); gyro_gain.j = pt.get<float>("Calibration.Gyro.Y.k", gyro_gain.j); gyro_gain.k = pt.get<float>("Calibration.Gyro.Z.k", gyro_gain.k); raw_gyro_bias.i = pt.get<float>("Calibration.Gyro.X.b", raw_gyro_bias.i); raw_gyro_bias.j = pt.get<float>("Calibration.Gyro.Y.b", raw_gyro_bias.j); raw_gyro_bias.k = pt.get<float>("Calibration.Gyro.Z.b", raw_gyro_bias.k); raw_gyro_variance = pt.get<float>("Calibration.Gyro.Variance", raw_gyro_variance); raw_gyro_drift = pt.get<float>("Calibration.Gyro.Drift", raw_gyro_drift); position_variance_exp_fit_a = pt.get<float>("Calibration.Position.VarianceExpFitA", position_variance_exp_fit_a); position_variance_exp_fit_b = pt.get<float>("Calibration.Position.VarianceExpFitB", position_variance_exp_fit_b); orientation_variance = pt.get<float>("Calibration.Orientation.Variance", orientation_variance); mean_update_time_delta = pt.get<float>("Calibration.Time.MeanUpdateTime", mean_update_time_delta); orientation_filter_type = pt.get<std::string>("OrientationFilter.FilterType", orientation_filter_type); position_filter_type = pt.get<std::string>("PositionFilter.FilterType", position_filter_type); max_velocity = pt.get<float>("PositionFilter.MaxVelocity", max_velocity); // Get the calibration direction for "down" identity_gravity_direction.i = pt.get<float>("Calibration.Identity.Gravity.X", identity_gravity_direction.i); identity_gravity_direction.j = pt.get<float>("Calibration.Identity.Gravity.Y", identity_gravity_direction.j); identity_gravity_direction.k = pt.get<float>("Calibration.Identity.Gravity.Z", identity_gravity_direction.k); // Read the tracking color tracking_color_id = static_cast<eCommonTrackingColorID>(readTrackingColor(pt)); } else { SERVER_LOG_WARNING("MorpheusHMDConfig") << "Config version " << version << " does not match expected version " << MorpheusHMDConfig::CONFIG_VERSION << ", Using defaults."; } }
void PSDualShock4ControllerConfig::ptree2config(const boost::property_tree::ptree &pt) { version = pt.get<int>("version", 0); if (version == PSDualShock4ControllerConfig::CONFIG_VERSION) { is_valid = pt.get<bool>("is_valid", false); prediction_time = pt.get<float>("prediction_time", 0.f); max_poll_failure_count = pt.get<long>("max_poll_failure_count", 100); // Use the current accelerometer values (constructor defaults) as the default values accelerometer_gain.i = pt.get<float>("Calibration.Accel.X.k", accelerometer_gain.i); accelerometer_gain.j = pt.get<float>("Calibration.Accel.Y.k", accelerometer_gain.j); accelerometer_gain.k = pt.get<float>("Calibration.Accel.Z.k", accelerometer_gain.k); accelerometer_bias.i = pt.get<float>("Calibration.Accel.X.b", accelerometer_bias.i); accelerometer_bias.j = pt.get<float>("Calibration.Accel.Y.b", accelerometer_bias.j); accelerometer_bias.k = pt.get<float>("Calibration.Accel.Z.b", accelerometer_bias.k); accelerometer_noise_radius = pt.get<float>("Calibration.Accel.NoiseRadius", accelerometer_noise_radius); accelerometer_variance= pt.get<float>("Calibration.Accel.Variance", accelerometer_variance); position_variance_exp_fit_a= pt.get<float>("Calibration.Position.VarianceExpFitA", position_variance_exp_fit_a); position_variance_exp_fit_b= pt.get<float>("Calibration.Position.VarianceExpFitB", position_variance_exp_fit_b); orientation_variance_exp_fit_a= pt.get<float>("Calibration.Orientation.VarianceExpFitA", orientation_variance_exp_fit_a); orientation_variance_exp_fit_b= pt.get<float>("Calibration.Orientation.VarianceExpFitB", orientation_variance_exp_fit_b); mean_update_time_delta= pt.get<float>("Calibration.Time.MeanUpdateTime", mean_update_time_delta); // Use the current gyroscope values (constructor defaults) as the default values gyro_gain= pt.get<float>("Calibration.Gyro.Gain", gyro_gain); gyro_variance= pt.get<float>("Calibration.Gyro.Variance", gyro_variance); gyro_drift= pt.get<float>("Calibration.Gyro.Drift", gyro_drift); // Get the orientation filter parameters orientation_filter_type= pt.get<std::string>("OrientationFilter.FilterType", orientation_filter_type); // Get the position filter parameters position_filter_type= pt.get<std::string>("PositionFilter.FilterType", position_filter_type); max_velocity= pt.get<float>("PositionFilter.MaxVelocity", max_velocity); position_use_linear_acceleration= pt.get<bool>("PositionFilter.UseLinearAcceleration", position_use_linear_acceleration); position_apply_gravity_mask= pt.get<bool>("PositionFilter.ApplyGravityMask", position_apply_gravity_mask); // Get shared filter parameters min_screen_projection_area = pt.get<float>("PoseFilter.MinScreenProjectionArea", min_screen_projection_area); // Get the calibration direction for "down" identity_gravity_direction.i= pt.get<float>("Calibration.Identity.Gravity.X", identity_gravity_direction.i); identity_gravity_direction.j= pt.get<float>("Calibration.Identity.Gravity.Y", identity_gravity_direction.j); identity_gravity_direction.k= pt.get<float>("Calibration.Identity.Gravity.Z", identity_gravity_direction.k); // Read the tracking color tracking_color_id = static_cast<eCommonTrackingColorID>(readTrackingColor(pt)); } else { SERVER_LOG_WARNING("PSDualShock4ControllerConfig") << "Config version " << version << " does not match expected version " << PSDualShock4ControllerConfig::CONFIG_VERSION << ", Using defaults."; } }
bool PS3EyeTracker::open(const DeviceEnumerator *enumerator) { const TrackerDeviceEnumerator *tracker_enumerator = static_cast<const TrackerDeviceEnumerator *>(enumerator); const char *cur_dev_path = tracker_enumerator->get_path(); bool bSuccess = false; if (getIsOpen()) { SERVER_LOG_WARNING("PS3EyeTracker::open") << "PS3EyeTracker(" << cur_dev_path << ") already open. Ignoring request."; bSuccess = true; } else { const int camera_index = tracker_enumerator->get_camera_index(); SERVER_LOG_INFO("PS3EyeTracker::open") << "Opening PS3EyeTracker(" << cur_dev_path << ", camera_index=" << camera_index << ")"; VideoCapture = new PSEyeVideoCapture(camera_index); if (VideoCapture->isOpened()) { CaptureData = new PSEyeCaptureData; USBDevicePath = enumerator->get_path(); bSuccess = true; } else { SERVER_LOG_ERROR("PS3EyeTracker::open") << "Failed to open PS3EyeTracker(" << cur_dev_path << ", camera_index=" << camera_index << ")"; close(); } } if (bSuccess) { std::string identifier = VideoCapture->getUniqueIndentifier(); std::string config_name = "PS3EyeTrackerConfig_"; config_name.append(identifier); cfg = PS3EyeTrackerConfig(config_name); cfg.load(); setExposure(cfg.exposure); setGain(cfg.gain); } return bSuccess; }
void PS3EyeTrackerConfig::ptree2config(const boost::property_tree::ptree &pt) { version = pt.get<int>("version", 0); if (version == PS3EyeTrackerConfig::CONFIG_VERSION) { is_valid = pt.get<bool>("is_valid", false); max_poll_failure_count = pt.get<long>("max_poll_failure_count", 100); exposure = pt.get<double>("exposure", 32); gain = pt.get<double>("gain", 32); focalLengthX = pt.get<double>("focalLengthX", 554.2563); focalLengthY = pt.get<double>("focalLengthY", 554.2563); principalX = pt.get<double>("principalX", 320.0); principalY = pt.get<double>("principalY", 240.0); hfov = pt.get<double>("hfov", 60.0); vfov = pt.get<double>("vfov", 45.0); zNear = pt.get<double>("zNear", 10.0); zFar = pt.get<double>("zFar", 200.0); fovSetting = static_cast<PS3EyeTrackerConfig::eFOVSetting>( pt.get<int>("fovSetting", PS3EyeTrackerConfig::eFOVSetting::BlueDot)); pose.Orientation.w = pt.get<float>("pose.orientation.w", 1.0); pose.Orientation.x = pt.get<float>("pose.orientation.x", 0.0); pose.Orientation.y = pt.get<float>("pose.orientation.y", 0.0); pose.Orientation.z = pt.get<float>("pose.orientation.z", 0.0); pose.Position.x = pt.get<float>("pose.position.x", 0.0); pose.Position.y = pt.get<float>("pose.position.y", 0.0); pose.Position.z = pt.get<float>("pose.position.z", 0.0); readColorPreset(pt, "", "magenta", &ColorPresets[eCommonTrackingColorID::Magenta], &k_default_color_presets[eCommonTrackingColorID::Magenta]); readColorPreset(pt, "", "cyan", &ColorPresets[eCommonTrackingColorID::Cyan], &k_default_color_presets[eCommonTrackingColorID::Cyan]); readColorPreset(pt, "", "yellow", &ColorPresets[eCommonTrackingColorID::Yellow], &k_default_color_presets[eCommonTrackingColorID::Yellow]); readColorPreset(pt, "", "red", &ColorPresets[eCommonTrackingColorID::Red], &k_default_color_presets[eCommonTrackingColorID::Red]); readColorPreset(pt, "", "green", &ColorPresets[eCommonTrackingColorID::Green], &k_default_color_presets[eCommonTrackingColorID::Green]); readColorPreset(pt, "", "blue", &ColorPresets[eCommonTrackingColorID::Blue], &k_default_color_presets[eCommonTrackingColorID::Blue]); } else { SERVER_LOG_WARNING("PS3EyeTrackerConfig") << "Config version " << version << " does not match expected version " << PS3EyeTrackerConfig::CONFIG_VERSION << ", Using defaults."; } }
bool PSDualShock4Controller::open( const DeviceEnumerator *enumerator) { const ControllerDeviceEnumerator *pEnum = static_cast<const ControllerDeviceEnumerator *>(enumerator); const char *cur_dev_path = pEnum->get_path(); bool success = false; if (getIsOpen()) { SERVER_LOG_WARNING("PSDualShock4Controller::open") << "PSDualShock4Controller(" << cur_dev_path << ") already open. Ignoring request."; success = true; } else { char cur_dev_serial_number[256]; SERVER_LOG_INFO("PSDualShock4Controller::open") << "Opening PSDualShock4Controller(" << cur_dev_path << ")"; if (pEnum->get_serial_number(cur_dev_serial_number, sizeof(cur_dev_serial_number))) { SERVER_LOG_INFO("PSDualShock4Controller::open") << " with serial_number: " << cur_dev_serial_number; } else { cur_dev_serial_number[0] = '\0'; SERVER_LOG_INFO("PSDualShock4Controller::open") << " with EMPTY serial_number"; } // Attempt to open the controller HIDDetails.vendor_id = pEnum->get_vendor_id(); HIDDetails.product_id = pEnum->get_product_id(); HIDDetails.Device_path = cur_dev_path; HIDDetails.Handle = hid_open_path(HIDDetails.Device_path.c_str()); if (HIDDetails.Handle != nullptr) // Controller was opened and has an index { // Don't block on hid report requests hid_set_nonblocking(HIDDetails.Handle, 1); /* -USB or Bluetooth Device- On my Mac, using bluetooth, cur_dev->path = Bluetooth_054c_03d5_779732e8 cur_dev->serial_number = 00-06-f7-97-32-e8 On my Mac, using USB, cur_dev->path = USB_054c_03d5_14100000 cur_dev->serial_number = "" (not null, just empty) On my Windows 10 box (different controller), using bluetooth cur_dev->path = \\?\hid#{00001124-0000-1000-8000-00805f9b34fb}_vid&0002054c_pid&05c4#8&217a4584&0&0000#{4d1e55b2-f16f-11cf-88cb-001111000030} cur_dev->serial_number = 1c666d2c8deb Using USB cur_dev->path = \\?\hid#vid_054c&pid_03d5&col01#6&7773e57&0&0000#{4d1e55b2-f16f-11cf-88cb-001111000030} cur_dev->serial_number = (null) */ // Bluetooth connected if (strlen(cur_dev_serial_number) > 0) { IsBluetooth = true; // Convert the serial number into a normalized bluetooth address of the form: "xx:xx:xx:xx:xx:xx" char szNormalizedControllerAddress[18]; ServerUtility::bluetooth_cstr_address_normalize( cur_dev_serial_number, true, ':', szNormalizedControllerAddress, sizeof(szNormalizedControllerAddress)); // Save the controller address as a std::string HIDDetails.Bt_addr = std::string(szNormalizedControllerAddress); // Get the (possibly cached) bluetooth address of the first bluetooth adapter if (!bluetooth_get_host_address(HIDDetails.Host_bt_addr)) { HIDDetails.Host_bt_addr= "00:00:00:00:00:00"; } success = true; } // USB Connected else { IsBluetooth = false; // Fetch the bluetooth host and controller addresses via USB HID report request success = getBTAddressesViaUSB(HIDDetails.Host_bt_addr, HIDDetails.Bt_addr); if (!success) { // If serial is still bad, maybe we have a disconnected // controller still showing up in hidapi SERVER_LOG_ERROR("PSDualShock4Controller::open") << "Failed to get bluetooth address of PSDualShock4Controller(" << cur_dev_path << ")"; } } if (success) { // Build a unique name for the config file using bluetooth address of the controller char szConfigSuffix[18]; ServerUtility::bluetooth_cstr_address_normalize( HIDDetails.Bt_addr.c_str(), true, '_', szConfigSuffix, sizeof(szConfigSuffix)); std::string config_name("dualshock4_"); config_name += szConfigSuffix; // Load the config file cfg = PSDualShock4ControllerConfig(config_name); cfg.load(); // Save it back out again in case any defaults changed cfg.save(); } // Reset the polling sequence counter NextPollSequenceNumber = 0; // Write out the initial controller state if (success && IsBluetooth) { bWriteStateDirty= true; writeDataOut(); } } else { SERVER_LOG_ERROR("PSDualShock4Controller::open") << "Failed to open PSDualShock4Controller(" << cur_dev_path << ")"; success = false; } } return success; }
bool PSNaviController::open( const DeviceEnumerator *enumerator) { const ControllerDeviceEnumerator *pEnum = static_cast<const ControllerDeviceEnumerator *>(enumerator); const char *cur_dev_path= pEnum->get_path(); bool success= false; if (getIsOpen()) { SERVER_LOG_WARNING("PSNaviController::open") << "PSNavoController(" << cur_dev_path << ") already open. Ignoring request."; success= true; } else { char cur_dev_serial_number[256]; SERVER_LOG_INFO("PSNaviController::open") << "Opening PSNaviController(" << cur_dev_path << ")"; if (pEnum->get_serial_number(cur_dev_serial_number, sizeof(cur_dev_serial_number))) { SERVER_LOG_INFO("PSNaviController::open") << " with serial_number: " << cur_dev_serial_number; } else { cur_dev_serial_number[0]= '\0'; SERVER_LOG_INFO("PSNaviController::open") << " with EMPTY serial_number"; } HIDDetails.Device_path = cur_dev_path; #ifdef _WIN32 HIDDetails.Device_path_addr = HIDDetails.Device_path; HIDDetails.Device_path_addr.replace(HIDDetails.Device_path_addr.find("&col01#"), 7, "&col02#"); HIDDetails.Device_path_addr.replace(HIDDetails.Device_path_addr.find("&0000#"), 6, "&0001#"); HIDDetails.Handle_addr = hid_open_path(HIDDetails.Device_path_addr.c_str()); hid_set_nonblocking(HIDDetails.Handle_addr, 1); #endif HIDDetails.Handle = hid_open_path(HIDDetails.Device_path.c_str()); hid_set_nonblocking(HIDDetails.Handle, 1); IsBluetooth = (strlen(cur_dev_serial_number) > 0); if (getIsOpen()) // Controller was opened and has an index { // Get the bluetooth address #ifndef _WIN32 // On my Mac, getting the bt feature report when connected via // bt crashes the controller. So we simply copy the serial number. // It gets modified in getBTAddress. // TODO: Copy this over anyway even in Windows. Check getBTAddress // comments for handling windows serial_number. // Once done, we can remove the ifndef above. std::string mbs(cur_dev_serial_number); HIDDetails.Bt_addr = mbs; #endif if (getBTAddress(HIDDetails.Host_bt_addr, HIDDetails.Bt_addr)) { // Load the config file std::string btaddr = HIDDetails.Bt_addr; std::replace(btaddr.begin(), btaddr.end(), ':', '_'); cfg = PSNaviControllerConfig(btaddr); cfg.load(); // TODO: Other startup. success= true; } else { // If serial is still bad, maybe we have a disconnected // controller still showing up in hidapi SERVER_LOG_ERROR("PSNaviController::open") << "Failed to get bluetooth address of PSNaviController(" << cur_dev_path << ")"; success= false; } // Reset the polling sequence counter NextPollSequenceNumber= 0; } else { SERVER_LOG_ERROR("PSNaviController::open") << "Failed to open PSNaviController(" << cur_dev_path << ")"; success= false; } } return success; }
bool DeviceTypeManager::update_connected_devices() { bool success = false; // Don't do any connection opening/closing until all pending bluetooth operations are finished if (can_update_connected_devices()) { const int maxDeviceCount = getMaxDevices(); ServerDeviceViewPtr *temp_device_list = new ServerDeviceViewPtr[maxDeviceCount]; bool bSendControllerUpdatedNotification = false; // Step 1 // See if any devices shuffled order OR if any new controllers were attached. // Migrate open devices to a new temp list in the order // that they appear in the device enumerator. { DeviceEnumerator *enumerator = allocate_device_enumerator(); while (enumerator->is_valid()) { // Find device index for the device with the matching device path int device_id = find_open_device_device_id(enumerator); // Existing device case (Most common) if (device_id != -1) { // Fetch the device from it's existing device slot ServerDeviceViewPtr existingDevice = getDeviceViewPtr(device_id); // Move it to the same slot in the temp list temp_device_list[device_id] = existingDevice; // Remove it from the previous list m_deviceViews[device_id] = ServerDeviceViewPtr(); } // New controller connected case else { int device_id = find_first_closed_device_device_id(); if (device_id != -1) { // Fetch the controller from it's existing controller slot ServerDeviceViewPtr existingDevice = getDeviceViewPtr(device_id); // Move it to the available slot existingDevice->setDeviceID(static_cast<int>(device_id)); temp_device_list[device_id] = existingDevice; // Remove it from the previous list m_deviceViews[device_id] = ServerDeviceViewPtr(); // Attempt to open the device if (existingDevice->open(enumerator)) { const char *device_type_name = CommonDeviceState::getDeviceTypeString(existingDevice->getDevice()->getDeviceType()); SERVER_LOG_INFO("DeviceTypeManager::update_connected_devices") << "Device device_id " << device_id << " (" << device_type_name << ") connected"; bSendControllerUpdatedNotification = true; } } else { SERVER_LOG_ERROR("DeviceTypeManager::update_connected_devices") << "Can't connect any more new devices. Too many open device."; break; } } enumerator->next(); } free_device_enumerator(enumerator); } // Step 2 // Close any remaining open controllers not listed in the device enumerator. // Copy over any closed controllers to the temp. for (int existing_device_id = 0; existing_device_id < maxDeviceCount; ++existing_device_id) { ServerDeviceViewPtr existingDevice = getDeviceViewPtr(existing_device_id); if (existingDevice) { // Any "open" controllers remaining in the old list need to be closed // since they no longer appear in the connected device list. // This probably shouldn't happen very often (at all?) as polling should catch // disconnected controllers first. if (existingDevice->getIsOpen()) { const char *device_type_name = CommonDeviceState::getDeviceTypeString(existingDevice->getDevice()->getDeviceType()); SERVER_LOG_WARNING("DeviceTypeManager::update_connected_devices") << "Closing device " << existing_device_id << " (" << device_type_name << ") since it's no longer in the device list."; existingDevice->close(); bSendControllerUpdatedNotification = true; } // Move it to the temp slot temp_device_list[existing_device_id] = existingDevice; // Remove it from the previous list m_deviceViews[existing_device_id] = ServerDeviceViewPtr(); } } // Step 3 // Copy the temp controller list back over top the original list for (int device_id = 0; device_id < maxDeviceCount; ++device_id) { m_deviceViews[device_id] = temp_device_list[device_id]; } if (bSendControllerUpdatedNotification) { send_device_list_changed_notification(); } delete[] temp_device_list; success = true; } return success; }
bool MorpheusHMD::open( const DeviceEnumerator *enumerator) { const HMDDeviceEnumerator *pEnum = static_cast<const HMDDeviceEnumerator *>(enumerator); const char *cur_dev_path = pEnum->get_path(); bool success = false; if (getIsOpen()) { SERVER_LOG_WARNING("MorpheusHMD::open") << "MorpheusHMD(" << cur_dev_path << ") already open. Ignoring request."; success = true; } else { SERVER_LOG_INFO("MorpheusHMD::open") << "Opening MorpheusHMD(" << cur_dev_path << ")."; USBContext->device_identifier = cur_dev_path; // Open the sensor interface using HIDAPI USBContext->sensor_device_path = pEnum->get_hid_hmd_enumerator()->get_interface_path(MORPHEUS_SENSOR_INTERFACE); USBContext->sensor_device_handle = hid_open_path(USBContext->sensor_device_path.c_str()); if (USBContext->sensor_device_handle != nullptr) { hid_set_nonblocking(USBContext->sensor_device_handle, 1); } // Open the command interface using libusb. // NOTE: Ideally we would use one usb library for both interfaces, but there are some complications. // A) The command interface uses the bulk transfer endpoint and HIDApi doesn't support that endpoint. // B) In Windows, libusb doesn't handle a high frequency of requests coming from two different threads well. // In this case, PS3EyeDriver is constantly sending bulk transfer requests in its own thread to get video frames. // If we started sending control transfer requests for the sensor data in the main thread at the same time // it can lead to a crash. It shouldn't, but this was a problem previously setting video feed properties // from the color config tool while a video feed was running. if (!cfg.disable_command_interface) { morpheus_open_usb_device(USBContext); } else { SERVER_LOG_WARNING("MorpheusHMD::open") << "Morpheus command interface is flagged as DISABLED."; } if (getIsOpen()) // Controller was opened and has an index { if (USBContext->usb_device_handle != nullptr) { if (morpheus_set_headset_power(USBContext, true)) { if (morpheus_enable_tracking(USBContext)) { morpheus_set_led_brightness(USBContext, _MorpheusLED_ALL, 0); } } } // Always save the config back out in case some defaults changed cfg.save(); // Reset the polling sequence counter NextPollSequenceNumber = 0; success = true; } else { SERVER_LOG_ERROR("MorpheusHMD::open") << "Failed to open MorpheusHMD(" << cur_dev_path << ")"; close(); } } return success; }
void OrientationFilter::update( const float delta_time, const OrientationSensorPacket &sensorPacket) { OrientationFilterPacket filterPacket; m_FilterSpace.convertSensorPacketToFilterPacket(sensorPacket, filterPacket); const Eigen::Quaternionf orientation_backup= m_FusionState->orientation; const Eigen::Quaternionf first_derivative_backup = m_FusionState->orientation_first_derivative; const Eigen::Quaternionf second_derivative_backup = m_FusionState->orientation_second_derivative; switch(m_FusionState->fusion_type) { case FusionTypeNone: break; case FusionTypePassThru: { const Eigen::Quaternionf &new_orientation= filterPacket.orientation; const Eigen::Quaternionf new_first_derivative= Eigen::Quaternionf( (new_orientation.coeffs() - m_FusionState->orientation.coeffs()) / delta_time); const Eigen::Quaternionf new_second_derivative = Eigen::Quaternionf( (new_first_derivative.coeffs() - m_FusionState->orientation_first_derivative.coeffs()) / delta_time); m_FusionState->orientation = new_orientation; m_FusionState->orientation_first_derivative = new_first_derivative; m_FusionState->orientation_second_derivative = new_second_derivative; } break; case FusionTypeMadgwickIMU: orientation_fusion_imu_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; case FusionTypeMadgwickMARG: orientation_fusion_madgwick_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; case FusionTypeComplementaryMARG: orientation_fusion_complementary_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState); break; } if (!eigen_quaternion_is_valid(m_FusionState->orientation)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!"; m_FusionState->orientation = orientation_backup; } if (!eigen_quaternion_is_valid(m_FusionState->orientation_first_derivative)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation first derivative is NaN!"; m_FusionState->orientation_first_derivative = first_derivative_backup; } if (!eigen_quaternion_is_valid(m_FusionState->orientation_second_derivative)) { SERVER_LOG_WARNING("OrientationFilter") << "Orientation second derivative is NaN!"; m_FusionState->orientation_second_derivative = second_derivative_backup; } }