bool
PSNaviController::setHostBluetoothAddress(const std::string &new_host_bt_addr)
{
bool success= false;
unsigned char bts[PSNAVI_BTADDR_SET_SIZE];
memset(bts, 0, sizeof(bts));
bts[0] = PSNavi_Req_SetBTAddr;
bts[1] = 0x01;
bts[2] = 0x00;
unsigned char addr[6];
if (stringToBTAddrUchar(new_host_bt_addr, addr, sizeof(addr)))
{
int res;
/* Copy 6 bytes from addr into bts[3]..bts[8] */
memcpy(&bts[3], addr, sizeof(addr));
/* _WIN32 only has move->handle_addr for getting bluetooth address. */
if (HIDDetails.Handle_addr)
{
res = hid_send_feature_report(HIDDetails.Handle_addr, bts, sizeof(bts));
}
else
{
res = hid_send_feature_report(HIDDetails.Handle, bts, sizeof(bts));
}
if (res == sizeof(bts))
{
success= true;
}
else
{
char hidapi_err_mbs[256];
bool valid_error_mesg= false;
if (HIDDetails.Handle_addr)
{
valid_error_mesg = hid_error_mbs(HIDDetails.Handle_addr, hidapi_err_mbs, sizeof(hidapi_err_mbs));
}
else
{
valid_error_mesg = hid_error_mbs(HIDDetails.Handle, hidapi_err_mbs, sizeof(hidapi_err_mbs));
}
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSNaviController::setBTAddress") << "HID ERROR: " << hidapi_err_mbs;
}
}
}
else
{
SERVER_LOG_ERROR("PSNaviController::setBTAddress") << "Malformed address: " << new_host_bt_addr;
}
return success;
}
bool
PSDualShock4Controller::writeDataOut()
{
bool bSuccess= true;
if (bWriteStateDirty)
{
const bool bLedIsOn = LedR != 0 || LedG != 0 || LedB != 0;
const bool bIsRumbleOn = RumbleRight != 0 || RumbleLeft != 0;
// Light bar color
OutData->led_r = LedR;
OutData->led_g = LedG;
OutData->led_b = LedB;
// Set off interval to 0% and the on interval to 100%.
// There are no discos in PSMoveService.
OutData->led_flash_on = bLedIsOn ? 0xff : 0x00;
OutData->led_flash_off = 0x00;
// a.k.a Soft Rumble Motor
OutData->rumble_right = RumbleRight;
// a.k.a Hard Rumble Motor
OutData->rumble_left = RumbleLeft;
// Keep writing state out until the desired LED and Rumble are 0
bWriteStateDirty = bLedIsOn || bIsRumbleOn;
// Unfortunately in windows simply writing to the HID device, via WriteFile() internally,
// doesn't appear to actually set the data on the controller (despite returning successfully).
// In the DS4 implementation they use the HidD_SetOutputReport() Win32 API call instead.
// Unfortunately HIDAPI doesn't have any equivalent call, so we have to make our own.
#ifdef _WIN32
int res = hid_set_output_report(HIDDetails.Handle, (unsigned char*)OutData, sizeof(PSDualShock4DataOutput));
#else
int res = hid_write(HIDDetails.Handle, (unsigned char*)OutData, sizeof(PSDualShock4DataOutput));
#endif
bSuccess = res > 0;
if (!bSuccess)
{
char szErrorMessage[256];
if (hid_error_mbs(HIDDetails.Handle, szErrorMessage, sizeof(szErrorMessage)))
{
SERVER_LOG_ERROR("PSDualShock4Controller::writeDataOut") << "HID ERROR: " << szErrorMessage;
}
}
}
return bSuccess;
}
bool
PSDualShock4Controller::setHostBluetoothAddress(const std::string &new_host_bt_addr)
{
bool success = false;
unsigned char bts[PSDS4_BTADDR_SET_SIZE];
memset(bts, 0, sizeof(bts));
bts[0] = PSDualShock4_USBReport_SetBTAddr;
unsigned char addr[6];
if (ServerUtility::bluetooth_string_address_to_bytes(new_host_bt_addr, addr, sizeof(addr)))
{
int res;
// Copy 6 bytes from addr into bts[1]..bts[6]
memcpy(&bts[1], addr, sizeof(addr));
// Copy the bluetooth link key index backwards into the bluetooth report
for (size_t key_byte_index = 0; key_byte_index < k_ps4_bluetooth_link_key_length; ++key_byte_index)
{
bts[7 + key_byte_index] = k_ps4_bluetooth_link_key[k_ps4_bluetooth_link_key_length - key_byte_index - 1];
}
/* _WIN32 only has move->handle_addr for getting bluetooth address. */
res = hid_send_feature_report(HIDDetails.Handle, bts, sizeof(bts));
if (res == sizeof(bts))
{
success = true;
}
else
{
char hidapi_err_mbs[256];
bool valid_error_mesg = false;
valid_error_mesg = hid_error_mbs(HIDDetails.Handle, hidapi_err_mbs, sizeof(hidapi_err_mbs));
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSDualShock4Controller::setBTAddress") << "HID ERROR: " << hidapi_err_mbs;
}
}
}
else
{
SERVER_LOG_ERROR("PSDualShock4Controller::setBTAddress") << "Malformed address: " << new_host_bt_addr;
}
return success;
}
bool
PSDualShock4Controller::getBTAddressesViaUSB(std::string& host, std::string& controller)
{
bool success = false;
int res;
unsigned char btg[PSDS4_BTADDR_GET_SIZE + 1];
unsigned char ctrl_char_buff[PSDS4_BTADDR_SIZE];
unsigned char host_char_buff[PSDS4_BTADDR_SIZE];
memset(btg, 0, sizeof(btg));
btg[0] = PSDualShock4_USBReport_GetBTAddr;
res = hid_get_feature_report(HIDDetails.Handle, btg, sizeof(btg));
if (res == sizeof(btg))
{
memcpy(host_char_buff, btg + 10, PSDS4_BTADDR_SIZE);
host = ServerUtility::bluetooth_byte_addr_to_string(host_char_buff);
memcpy(ctrl_char_buff, btg + 1, PSDS4_BTADDR_SIZE);
controller = ServerUtility::bluetooth_byte_addr_to_string(ctrl_char_buff);
success = true;
}
else
{
char hidapi_err_mbs[256];
bool valid_error_mesg = false;
valid_error_mesg = hid_error_mbs(HIDDetails.Handle, hidapi_err_mbs, sizeof(hidapi_err_mbs));
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSDualShock4Controller::getBTAddress") << "HID ERROR: " << hidapi_err_mbs;
}
}
return success;
}
IControllerInterface::ePollResult
PSDualShock4Controller::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(PSDualShock4DataInput));
if (res == 0)
{
//SERVER_LOG_WARNING("PSDualShock4Controller::readDataIn") << "Read Bytes: " << res;
// 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("PSDualShock4Controller::readDataIn") << "HID ERROR: " << hidapi_err_mbs;
}
result = IControllerInterface::_PollResultFailure;
// No more data available. Stop iterating.
break;
}
else
{
//SERVER_LOG_WARNING("PSDualShock4Controller::readDataIn") << "Read Bytes: " << res;
// New data available. Keep iterating.
result = IControllerInterface::_PollResultSuccessNewData;
}
// https://github.com/nitsch/moveonpc/wiki/Input-report
PSDualShock4ControllerState newState;
// Increment the sequence for every new polling packet
newState.PollSequenceNumber = NextPollSequenceNumber;
++NextPollSequenceNumber;
// Smush the button state into one unsigned 32-bit variable
newState.AllButtons =
(((unsigned int)InData->buttons3.raw & 0x3) << 16) | // Get the 1st two bits of buttons: [0|0|0|0|0|0|PS|TPad]
(unsigned int)(InData->buttons2.raw << 8) | // [R3|L3|Option|Share|R2|L2|R1|L1]
((unsigned int)InData->buttons1.raw & 0xF0); // Mask out the dpad enum (1st four bits): [tri|cir|x|sq|0|0|0|0]
// Converts the dpad enum to independent bit flags
{
ePSDualShock4_DPad dpad_enum = static_cast<ePSDualShock4_DPad>(InData->buttons1.raw & 0xF);
unsigned int dpad_bits= 0;
switch (dpad_enum)
{
case ePSDualShock4_DPad::PSDualShock4DPad_N:
dpad_bits |= ePSDS4_Button::Btn_DPAD_UP;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_NE:
dpad_bits |= ePSDS4_Button::Btn_DPAD_UP;
dpad_bits |= ePSDS4_Button::Btn_DPAD_RIGHT;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_E:
dpad_bits |= ePSDS4_Button::Btn_DPAD_RIGHT;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_SE:
dpad_bits |= ePSDS4_Button::Btn_DPAD_DOWN;
dpad_bits |= ePSDS4_Button::Btn_DPAD_RIGHT;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_S:
dpad_bits |= ePSDS4_Button::Btn_DPAD_DOWN;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_SW:
dpad_bits |= ePSDS4_Button::Btn_DPAD_DOWN;
dpad_bits |= ePSDS4_Button::Btn_DPAD_LEFT;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_W:
dpad_bits |= ePSDS4_Button::Btn_DPAD_LEFT;
break;
case ePSDualShock4_DPad::PSDualShock4DPad_NW:
dpad_bits |= ePSDS4_Button::Btn_DPAD_UP;
dpad_bits |= ePSDS4_Button::Btn_DPAD_LEFT;
break;
}
// Append in the DPad bits
newState.AllButtons |= (dpad_bits & 0xf);
}
// Update the button state enum
{
unsigned int lastButtons = ControllerStates.empty() ? 0 : ControllerStates.back().AllButtons;
newState.DPad_Up = getButtonState(newState.AllButtons, lastButtons, Btn_DPAD_UP);
newState.DPad_Down = getButtonState(newState.AllButtons, lastButtons, Btn_DPAD_DOWN);
newState.DPad_Left = getButtonState(newState.AllButtons, lastButtons, Btn_DPAD_LEFT);
newState.DPad_Right = getButtonState(newState.AllButtons, lastButtons, Btn_DPAD_RIGHT);
newState.Square = getButtonState(newState.AllButtons, lastButtons, Btn_SQUARE);
newState.Cross = getButtonState(newState.AllButtons, lastButtons, Btn_CROSS);
newState.Circle = getButtonState(newState.AllButtons, lastButtons, Btn_CIRCLE);
newState.Triangle = getButtonState(newState.AllButtons, lastButtons, Btn_TRIANGLE);
newState.L1 = getButtonState(newState.AllButtons, lastButtons, Btn_L1);
newState.R1 = getButtonState(newState.AllButtons, lastButtons, Btn_R1);
newState.L2 = getButtonState(newState.AllButtons, lastButtons, Btn_L2);
newState.R2 = getButtonState(newState.AllButtons, lastButtons, Btn_R2);
newState.Share = getButtonState(newState.AllButtons, lastButtons, Btn_SHARE);
newState.Options = getButtonState(newState.AllButtons, lastButtons, Btn_OPTION);
newState.L3 = getButtonState(newState.AllButtons, lastButtons, Btn_L3);
newState.R3 = getButtonState(newState.AllButtons, lastButtons, Btn_R3);
newState.PS = getButtonState(newState.AllButtons, lastButtons, Btn_PS);
newState.TrackPadButton = getButtonState(newState.AllButtons, lastButtons, Btn_TPAD);
}
// Remap the analog sticks from [0,255] -> [-1.f,1.f]
newState.LeftAnalogX= ((static_cast<float>(InData->left_stick_x) / 255.f) - 0.5f) * 2.f;
newState.LeftAnalogY = ((static_cast<float>(InData->left_stick_y) / 255.f) - 0.5f) * 2.f;
newState.RightAnalogX = ((static_cast<float>(InData->right_stick_x) / 255.f) - 0.5f) * 2.f;
newState.RightAnalogY = ((static_cast<float>(InData->right_stick_y) / 255.f) - 0.5f) * 2.f;
// Remap the analog triggers from [0,255] -> [0.f,1.f]
newState.LeftTrigger = static_cast<float>(InData->left_trigger) / 255.f;
newState.RightTrigger = static_cast<float>(InData->right_trigger) / 255.f;
// Processes the IMU data
{
// Piece together the 12-bit accelerometer data
short raw_accelX = static_cast<short>((InData->accel_x[1] << 8) | InData->accel_x[0]) >> 4;
short raw_accelY = static_cast<short>((InData->accel_y[1] << 8) | InData->accel_y[0]) >> 4;
short raw_accelZ = static_cast<short>((InData->accel_z[1] << 8) | InData->accel_z[0]) >> 4;
// Piece together the 16-bit gyroscope data
short raw_gyroX = static_cast<short>((InData->gyro_x[1] << 8) | InData->gyro_x[0]);
short raw_gyroY = static_cast<short>((InData->gyro_y[1] << 8) | InData->gyro_y[0]);
short raw_gyroZ = static_cast<short>((InData->gyro_z[1] << 8) | InData->gyro_z[0]);
// Save the raw accelerometer values
newState.RawAccelerometer[0] = static_cast<int>(raw_accelX);
newState.RawAccelerometer[1] = static_cast<int>(raw_accelY);
newState.RawAccelerometer[2] = static_cast<int>(raw_accelZ);
// Save the raw gyro values
newState.RawGyro[0] = static_cast<int>(raw_gyroX);
newState.RawGyro[1] = static_cast<int>(raw_gyroY);
newState.RawGyro[2] = static_cast<int>(raw_gyroZ);
// calibrated_acc= raw_acc*acc_gain + acc_bias
newState.CalibratedAccelerometer.i =
static_cast<float>(newState.RawAccelerometer[0]) * cfg.accelerometer_gain.i
+ cfg.accelerometer_bias.i;
newState.CalibratedAccelerometer.j =
static_cast<float>(newState.RawAccelerometer[1]) * cfg.accelerometer_gain.j
+ cfg.accelerometer_bias.j;
newState.CalibratedAccelerometer.k =
static_cast<float>(newState.RawAccelerometer[2]) * cfg.accelerometer_gain.k
+ cfg.accelerometer_bias.k;
// calibrated_gyro= raw_gyro*gyro_gain + gyro_bias
newState.CalibratedGyro.i = static_cast<float>(newState.RawGyro[0]) * cfg.gyro_gain;
newState.CalibratedGyro.j = static_cast<float>(newState.RawGyro[1]) * cfg.gyro_gain;
newState.CalibratedGyro.k = static_cast<float>(newState.RawGyro[2]) * cfg.gyro_gain;
}
// Sequence and timestamp
newState.RawSequence = InData->buttons3.state.counter;
newState.RawTimeStamp = InData->timestamp;
// Convert the 0-10 battery level into the batter level
switch (InData->batteryLevel)
{
case 0:
newState.Battery = CommonControllerState::BatteryLevel::Batt_CHARGING;
break;
case 1:
newState.Battery = CommonControllerState::BatteryLevel::Batt_MIN;
break;
case 2:
case 3:
newState.Battery = CommonControllerState::BatteryLevel::Batt_20Percent;
break;
case 4:
case 5:
newState.Battery = CommonControllerState::BatteryLevel::Batt_40Percent;
break;
case 6:
case 7:
newState.Battery = CommonControllerState::BatteryLevel::Batt_60Percent;
break;
case 8:
case 9:
newState.Battery = CommonControllerState::BatteryLevel::Batt_80Percent;
break;
case 10:
default:
newState.Battery = CommonControllerState::BatteryLevel::Batt_MAX;
break;
}
// Make room for new entry if at the max queue size
if (ControllerStates.size() >= PSDS4_STATE_BUFFER_MAX)
{
ControllerStates.erase(ControllerStates.begin(),
ControllerStates.begin() + ControllerStates.size() - PSDS4_STATE_BUFFER_MAX);
}
ControllerStates.push_back(newState);
}
// Update recurrent writes on a regular interval
{
std::chrono::time_point<std::chrono::high_resolution_clock> now = std::chrono::high_resolution_clock::now();
// See if it's time to update the LED/rumble state
std::chrono::duration<double, std::milli> led_update_diff = now - lastWriteStateTime;
if (led_update_diff.count() >= PSDS4_WRITE_DATA_INTERVAL_MS)
{
writeDataOut();
lastWriteStateTime = now;
}
}
}
return result;
}
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
PSNaviController::getBTAddress(std::string& host, std::string& controller)
{
bool success = false;
if (IsBluetooth && !controller.empty())
{
std::replace(controller.begin(), controller.end(), '-', ':');
std::transform(controller.begin(), controller.end(), controller.begin(), ::tolower);
//TODO: If the third entry is not : and length is PSNAVI_BTADDR_SIZE
// std::stringstream ss;
// ss << controller.substr(0, 2) << ":" << controller.substr(2, 2) <<
// ":" << controller.substr(4, 2) << ":" << controller.substr(6, 2) <<
// ":" << controller.substr(8, 2) << ":" << controller.substr(10, 2);
// controller = ss.str();
success = true;
}
else
{
int res;
unsigned char btg[PSNAVI_BTADDR_GET_SIZE];
unsigned char ctrl_char_buff[PSNAVI_BTADDR_SIZE];
memset(btg, 0, sizeof(btg));
btg[0] = PSNavi_Req_GetBTAddr;
/* _WIN32 only has move->handle_addr for getting bluetooth address. */
if (HIDDetails.Handle_addr) {
res = hid_get_feature_report(HIDDetails.Handle_addr, btg, sizeof(btg));
}
else {
res = hid_get_feature_report(HIDDetails.Handle, btg, sizeof(btg));
}
if (res == sizeof(btg)) {
memcpy(ctrl_char_buff, btg + 2, PSNAVI_BTADDR_SIZE);
controller = btAddrUcharToString(ctrl_char_buff);
success = true;
}
else
{
char hidapi_err_mbs[256];
bool valid_error_mesg= false;
if (HIDDetails.Handle_addr)
{
valid_error_mesg = hid_error_mbs(HIDDetails.Handle_addr, hidapi_err_mbs, sizeof(hidapi_err_mbs));
}
else
{
valid_error_mesg = hid_error_mbs(HIDDetails.Handle, hidapi_err_mbs, sizeof(hidapi_err_mbs));
}
if (valid_error_mesg)
{
SERVER_LOG_ERROR("PSNaviController::getBTAddress") << "HID ERROR: " << hidapi_err_mbs;
}
}
}
return success;
}
