PS3EyeTracker::~PS3EyeTracker()
{
if (getIsOpen())
{
SERVER_LOG_ERROR("~PS3EyeTracker") << "Tracker deleted without calling close() first!";
}
}
void
ServerDeviceView::close()
{
if (getIsOpen())
{
getDevice()->close();
free_device_interface();
}
}
PSDualShock4Controller::~PSDualShock4Controller()
{
if (getIsOpen())
{
SERVER_LOG_ERROR("~PSDualShock4Controller") << "Controller deleted without calling close() first!";
}
delete InData;
}
XERCES_CPP_NAMESPACE_BEGIN
// ---------------------------------------------------------------------------
// BinFileOutputStream: Constructors and Destructor
// ---------------------------------------------------------------------------
BinFileOutputStream::~BinFileOutputStream()
{
if (getIsOpen())
XMLPlatformUtils::closeFile(fSource, fMemoryManager);
}
MorpheusHMD::~MorpheusHMD()
{
if (getIsOpen())
{
SERVER_LOG_ERROR("~MorpheusHMD") << "HMD deleted without calling close() first!";
}
delete InData;
delete USBContext;
}
void close()
{
if (getIsOpen())
{
#if RIO_PLATFORM_POSIX
fclose(file);
file = nullptr;
#elif RIO_PLATFORM_WINDOWS
CloseHandle(file);
file = INVALID_HANDLE_VALUE;
#endif // RIO_PLATFORM_
}
}
bool
PSDualShock4Controller::getTrackingColorID(eCommonTrackingColorID &out_tracking_color_id) const
{
bool bSuccess = false;
if (getIsOpen() && getIsBluetooth())
{
out_tracking_color_id = cfg.tracking_color_id;
bSuccess = true;
}
return bSuccess;
}
bool
PSDualShock4Controller::setTrackingColorID(const eCommonTrackingColorID tracking_color_id)
{
bool bSuccess = false;
if (getIsOpen() && getIsBluetooth())
{
cfg.tracking_color_id = tracking_color_id;
cfg.save();
bSuccess = true;
}
return bSuccess;
}
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;
}
IDeviceInterface::ePollResult PS3EyeTracker::poll()
{
IDeviceInterface::ePollResult result = IDeviceInterface::_PollResultFailure;
if (getIsOpen())
{
if (!VideoCapture->grab() ||
!VideoCapture->retrieve(CaptureData->frame, cv::CAP_OPENNI_BGR_IMAGE))
{
// Device still in valid state
result = IControllerInterface::_PollResultSuccessNoData;
}
else
{
// New data available. Keep iterating.
result = IControllerInterface::_PollResultSuccessNewData;
}
{
PS3EyeTrackerState newState;
// TODO: Process the frame and extract the blobs
// Increment the sequence for every new polling packet
newState.PollSequenceNumber = NextPollSequenceNumber;
++NextPollSequenceNumber;
// Make room for new entry if at the max queue size
//###bwalker $TODO Make this a fixed size circular buffer
if (TrackerStates.size() >= PS3EYE_STATE_BUFFER_MAX)
{
TrackerStates.erase(TrackerStates.begin(), TrackerStates.begin() + TrackerStates.size() - PS3EYE_STATE_BUFFER_MAX);
}
TrackerStates.push_back(newState);
}
}
return result;
}
void PSDualShock4Controller::close()
{
if (getIsOpen())
{
SERVER_LOG_INFO("PSDualShock4Controller::close") << "Closing PSDualShock4Controller(" << HIDDetails.Device_path << ")";
if (HIDDetails.Handle != nullptr)
{
if (IsBluetooth)
{
clearAndWriteDataOut();
}
hid_close(HIDDetails.Handle);
HIDDetails.Handle = nullptr;
}
}
else
{
SERVER_LOG_INFO("PSDualShock4Controller::close") << "PSDualShock4Controller(" << HIDDetails.Device_path << ") already closed. Ignoring request.";
}
}
void PSNaviController::close()
{
if (getIsOpen())
{
SERVER_LOG_INFO("PSNaviController::close") << "Closing PSNaviController(" << HIDDetails.Device_path << ")";
if (HIDDetails.Handle != nullptr)
{
hid_close(HIDDetails.Handle);
HIDDetails.Handle= nullptr;
}
if (HIDDetails.Handle_addr != nullptr)
{
hid_close(HIDDetails.Handle_addr);
HIDDetails.Handle_addr= nullptr;
}
}
else
{
SERVER_LOG_INFO("PSNaviController::close") << "PSNaviController(" << HIDDetails.Device_path << ") already closed. Ignoring request.";
}
}
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 PS3EyeTracker::getIsReadyToPoll() const
{
return getIsOpen();
}
IControllerInterface::ePollResult
PSNaviController::poll()
{
IControllerInterface::ePollResult result= IControllerInterface::_PollResultFailure;
if (!getIsBluetooth())
{
// Don't bother polling when connected via usb
result = IControllerInterface::_PollResultSuccessNoData;
}
else if (getIsOpen())
{
static const int k_max_iterations= 32;
for (int iteration= 0; iteration < k_max_iterations; ++iteration)
{
// Attempt to read the next update packet from the controller
int res = hid_read(HIDDetails.Handle, (unsigned char*)InData, sizeof(PSNaviDataInput));
if (res == 0)
{
// Device still in valid state
result= (iteration == 0)
? IControllerInterface::_PollResultSuccessNoData
: IControllerInterface::_PollResultSuccessNewData;
// No more data available. Stop iterating.
break;
}
else if (res < 0)
{
char hidapi_err_mbs[256];
bool valid_error_mesg = hid_error_mbs(HIDDetails.Handle, hidapi_err_mbs, sizeof(hidapi_err_mbs));
// Device no longer in valid state.
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSMoveController::readDataIn") << "HID ERROR: " << hidapi_err_mbs;
}
result= IControllerInterface::_PollResultFailure;
// No more data available. Stop iterating.
break;
}
else
{
// New data available. Keep iterating.
result= IControllerInterface::_PollResultFailure;
}
// https://github.com/nitsch/moveonpc/wiki/Input-report
PSNaviControllerState newState;
// Increment the sequence for every new polling packet
newState.PollSequenceNumber= NextPollSequenceNumber;
++NextPollSequenceNumber;
// Buttons
newState.AllButtons =
(InData->buttons2) | // |-|L2|L1|-|-|Circle|-|Cross
(InData->buttons1 << 8) | // |Left|Down|Right|Up|-|-|L3|-
((InData->buttons3 & 0x01) << 16); // |-|-|-|-|-|-|-|PS
unsigned int lastButtons = ControllerStates.empty() ? 0 : ControllerStates.back().AllButtons;
newState.Circle = getButtonState(newState.AllButtons, lastButtons, Btn_CIRCLE);
newState.Cross = getButtonState(newState.AllButtons, lastButtons, Btn_CROSS);
newState.PS = getButtonState(newState.AllButtons, lastButtons, Btn_PS);
newState.Trigger = InData->analog_l2;
newState.Stick_XAxis= InData->stick_xaxis;
newState.Stick_YAxis= InData->stick_yaxis;
// Other
newState.Battery = static_cast<CommonControllerState::BatteryLevel>(InData->battery);
// Make room for new entry if at the max queue size
if (ControllerStates.size() >= PSNAVI_STATE_BUFFER_MAX)
{
ControllerStates.erase(ControllerStates.begin(),
ControllerStates.begin()+ControllerStates.size()-PSNAVI_STATE_BUFFER_MAX);
}
ControllerStates.push_back(newState);
}
}
return result;
}
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;
}
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;
}
IControllerInterface::ePollResult
MorpheusHMD::poll()
{
IHMDInterface::ePollResult result = IHMDInterface::_PollResultFailure;
if (getIsOpen())
{
static const int k_max_iterations = 32;
for (int iteration = 0; iteration < k_max_iterations; ++iteration)
{
// Attempt to read the next update packet from the controller
int res = hid_read(USBContext->sensor_device_handle, (unsigned char*)InData, sizeof(MorpheusSensorData));
if (res == 0)
{
// Device still in valid state
result = (iteration == 0)
? IHMDInterface::_PollResultSuccessNoData
: IHMDInterface::_PollResultSuccessNewData;
// No more data available. Stop iterating.
break;
}
else if (res < 0)
{
char hidapi_err_mbs[256];
bool valid_error_mesg =
ServerUtility::convert_wcs_to_mbs(hid_error(USBContext->sensor_device_handle), hidapi_err_mbs, sizeof(hidapi_err_mbs));
// Device no longer in valid state.
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSMoveController::readDataIn") << "HID ERROR: " << hidapi_err_mbs;
}
result = IHMDInterface::_PollResultFailure;
// No more data available. Stop iterating.
break;
}
else
{
// New data available. Keep iterating.
result = IHMDInterface::_PollResultSuccessNewData;
}
// https://github.com/hrl7/node-psvr/blob/master/lib/psvr.js
MorpheusHMDState newState;
// Increment the sequence for every new polling packet
newState.PollSequenceNumber = NextPollSequenceNumber;
++NextPollSequenceNumber;
// Processes the IMU data
newState.parse_data_input(&cfg, InData);
// Make room for new entry if at the max queue size
if (HMDStates.size() >= MORPHEUS_HMD_STATE_BUFFER_MAX)
{
HMDStates.erase(HMDStates.begin(), HMDStates.begin() + HMDStates.size() - MORPHEUS_HMD_STATE_BUFFER_MAX);
}
HMDStates.push_back(newState);
}
}
return result;
}
bool
MorpheusHMD::getIsReadyToPoll() const
{
return (getIsOpen());
}
bool
PSDualShock4Controller::getIsReadyToPoll() const
{
return (getIsOpen() && getIsBluetooth());
}
bool
PSNaviController::getIsReadyToPoll() const
{
return (getIsOpen() && getIsBluetooth());
}
BinFileInputStream::~BinFileInputStream()
{
if (getIsOpen())
XMLPlatformUtils::closeFile(fSource, fMemoryManager);
}
bool
ServerDeviceView::matchesDeviceEnumerator(const DeviceEnumerator *enumerator) const
{
return getIsOpen() && getDevice()->matchesDeviceEnumerator(enumerator);
}
