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;
}
}
