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;
}
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;
}
bool
PSDualShock4Controller::getIsReadyToPoll() const
{
return (getIsOpen() && getIsBluetooth());
}
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::getIsReadyToPoll() const
{
return (getIsOpen() && getIsBluetooth());
}
