cemu-project_Mirror/Cemu
1
0
Fork 0
mirror of https://github.com/cemu-project/Cemu.git synced 2025-04-28 22:39:26 -04:00
Code Issues Projects Releases Packages Wiki Activity
Cemu/src/input/api/Wiimote/WiimoteControllerProvider.cpp
capitalistspz dd0af0a56f
Linux: Allow connecting Wiimotes via L2CAP (#1353)
2024-12-07 12:02:40 +01:00

1092 lines
32 KiB
C++
Raw Blame History

#include "input/api/Wiimote/WiimoteControllerProvider.h"
#include "input/api/Wiimote/NativeWiimoteController.h"
#include "input/api/Wiimote/WiimoteMessages.h"
#ifdef HAS_HIDAPI
#include "input/api/Wiimote/hidapi/HidapiWiimote.h"
#endif
#ifdef HAS_BLUEZ
#include "input/api/Wiimote/l2cap/L2CapWiimote.h"
#endif
#include <numbers>
#include <queue>
WiimoteControllerProvider::WiimoteControllerProvider()
: m_running(true)
{
m_reader_thread = std::thread(&WiimoteControllerProvider::reader_thread, this);
m_writer_thread = std::thread(&WiimoteControllerProvider::writer_thread, this);
m_connectionThread = std::thread(&WiimoteControllerProvider::connectionThread, this);
}
WiimoteControllerProvider::~WiimoteControllerProvider()
{
if (m_running)
{
m_running = false;
m_writer_thread.join();
m_reader_thread.join();
m_connectionThread.join();
}
}
std::vector<std::shared_ptr<ControllerBase>> WiimoteControllerProvider::get_controllers()
{
m_connectedDeviceMutex.lock();
auto devices = m_connectedDevices;
m_connectedDeviceMutex.unlock();
std::scoped_lock lock(m_device_mutex);
for (auto& device : devices)
{
const auto writeable = device->write_data({kStatusRequest, 0x00});
if (!writeable)
continue;
bool isDuplicate = false;
ssize_t lowestReplaceableIndex = -1;
for (ssize_t i = m_wiimotes.size() - 1; i >= 0; --i)
{
const auto& wiimoteDevice = m_wiimotes[i].device;
if (wiimoteDevice)
{
if (*wiimoteDevice == *device)
{
isDuplicate = true;
break;
}
continue;
}
lowestReplaceableIndex = i;
}
if (isDuplicate)
continue;
if (lowestReplaceableIndex != -1)
m_wiimotes.replace(lowestReplaceableIndex, std::make_unique<Wiimote>(device));
else
m_wiimotes.push_back(std::make_unique<Wiimote>(device));
}
std::vector<std::shared_ptr<ControllerBase>> result;
result.reserve(m_wiimotes.size());
for (size_t i = 0; i < m_wiimotes.size(); ++i)
{
result.emplace_back(std::make_shared<NativeWiimoteController>(i));
}
return result;
}
bool WiimoteControllerProvider::is_connected(size_t index)
{
std::shared_lock lock(m_device_mutex);
return index < m_wiimotes.size() && m_wiimotes[index].device;
}
bool WiimoteControllerProvider::is_registered_device(size_t index)
{
std::shared_lock lock(m_device_mutex);
return index < m_wiimotes.size();
}
void WiimoteControllerProvider::set_rumble(size_t index, bool state)
{
std::shared_lock lock(m_device_mutex);
if (index >= m_wiimotes.size())
return;
m_wiimotes[index].rumble = state;
lock.unlock();
send_packet(index, { kStatusRequest, 0x00 });
}
void WiimoteControllerProvider::request_status(size_t index)
{
send_packet(index, {kStatusRequest, 0x00});
}
void WiimoteControllerProvider::set_led(size_t index, size_t player_index)
{
uint8 mask = 0;
mask |= 1 << (4 + (player_index % 4));
if (player_index >= 4)
mask |= 1 << (4 + ((player_index - 3) % 4));
send_packet(index, {kLED, mask});
}
uint32 WiimoteControllerProvider::get_packet_delay(size_t index)
{
std::shared_lock lock(m_device_mutex);
if (index < m_wiimotes.size())
{
return m_wiimotes[index].data_delay;
}
return kDefaultPacketDelay;
}
void WiimoteControllerProvider::set_packet_delay(size_t index, uint32 delay)
{
std::shared_lock lock(m_device_mutex);
if (index < m_wiimotes.size())
{
m_wiimotes[index].data_delay = delay;
}
}
WiimoteControllerProvider::WiimoteState WiimoteControllerProvider::get_state(size_t index)
{
std::shared_lock lock(m_device_mutex);
if (index < m_wiimotes.size())
{
std::shared_lock data_lock(m_wiimotes[index].mutex);
return m_wiimotes[index].state;
}
return {};
}
void WiimoteControllerProvider::connectionThread()
{
SetThreadName("Wiimote-connect");
while (m_running.load(std::memory_order_relaxed))
{
std::vector<WiimoteDevicePtr> devices;
#ifdef HAS_HIDAPI
const auto& hidDevices = HidapiWiimote::get_devices();
std::ranges::move(hidDevices, std::back_inserter(devices));
#endif
#ifdef HAS_BLUEZ
const auto& l2capDevices = L2CapWiimote::get_devices();
std::ranges::move(l2capDevices, std::back_inserter(devices));
#endif
{
std::scoped_lock lock(m_connectedDeviceMutex);
m_connectedDevices.clear();
std::ranges::move(devices, std::back_inserter(m_connectedDevices));
}
std::this_thread::sleep_for(std::chrono::seconds(2));
}
}
void WiimoteControllerProvider::reader_thread()
{
SetThreadName("Wiimote-reader");
std::chrono::steady_clock::time_point lastCheck = {};
while (m_running.load(std::memory_order_relaxed))
{
const auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::seconds>(now - lastCheck) > std::chrono::milliseconds(500))
{
// check for new connected wiimotes
get_controllers();
lastCheck = std::chrono::steady_clock::now();
}
bool receivedAnyPacket = false;
std::shared_lock lock(m_device_mutex);
for (size_t index = 0; index < m_wiimotes.size(); ++index)
{
auto& wiimote = m_wiimotes[index];
if (!wiimote.device)
continue;
const auto read_data = wiimote.device->read_data();
if (!read_data)
{
wiimote.device.reset();
continue;
}
if (read_data->empty())
continue;
receivedAnyPacket = true;
std::shared_lock read_lock(wiimote.mutex);
WiimoteState new_state = wiimote.state;
read_lock.unlock();
bool update_report = false;
const uint8* data = read_data->data();
const auto id = (InputReportId)*data;
++data;
switch (id)
{
case kStatus:
{
cemuLog_logDebug(LogType::Force,"WiimoteControllerProvider::read_thread: kStatus");
new_state.buttons = (*(uint16*)data) & (~0x60E0);
data += 2;
new_state.flags = *data;
++data;
data += 2; // skip zeroes
new_state.battery_level = *data;
++data;
new_state.ir_camera.mode = set_ir_camera(index, true);
if(!new_state.m_calibrated)
calibrate(index);
if(!new_state.m_motion_plus)
detect_motion_plus(index);
if (HAS_FLAG(new_state.flags, kExtensionConnected))
{
cemuLog_logDebug(LogType::Force,"Extension flag is set");
if(new_state.m_extension.index() == 0)
request_extension(index);
}
else
{
new_state.m_extension = {};
}
update_report = true;
}
break;
case kRead:
{
cemuLog_logDebug(LogType::Force,"WiimoteControllerProvider::read_thread: kRead");
new_state.buttons = (*(uint16*)data) & (~0x60E0);
data += 2;
const uint8 error_flag = *data & 0xF, size = (*data >> 4) + 1;
++data;
if (error_flag)
{
// 7 means that wiimote is already enabled or not available
cemuLog_logDebug(LogType::Force,"Received error on data read {:#x}", error_flag);
continue;
}
auto address = *(betype<uint16>*)data;
data += 2;
if (address == (kRegisterCalibration & 0xFFFF))
{
cemuLog_logDebug(LogType::Force,"Calibration received");
cemu_assert(size == 8);
new_state.m_calib_acceleration.zero.x = (uint16)*data << 2;
++data;
new_state.m_calib_acceleration.zero.y = (uint16)*data << 2;
++data;
new_state.m_calib_acceleration.zero.z = (uint16)*data << 2;
++data;
// --XXYYZZ
new_state.m_calib_acceleration.zero.x |= (*data >> 4) & 0x3; // 5|4 -> 1|0
new_state.m_calib_acceleration.zero.y |= (*data >> 2) & 0x3; // 3|4 -> 1|0
new_state.m_calib_acceleration.zero.z |= *data & 0x3;
++data;
new_state.m_calib_acceleration.gravity.x = (uint16)*data << 2;
++data;
new_state.m_calib_acceleration.gravity.y = (uint16)*data << 2;
++data;
new_state.m_calib_acceleration.gravity.z = (uint16)*data << 2;
++data;
new_state.m_calib_acceleration.gravity.x |= (*data >> 4) & 0x3; // 5|4 -> 1|0
new_state.m_calib_acceleration.gravity.y |= (*data >> 2) & 0x3; // 3|4 -> 1|0
new_state.m_calib_acceleration.gravity.z |= *data & 0x3;
++data;
new_state.m_calibrated = true;
}
else if (address == (kRegisterExtensionType & 0xFFFF))
{
if (size == 0xf)
{
cemuLog_logDebug(LogType::Force,"Extension type received but no extension connected");
continue;
}
cemu_assert(size == 6);
auto be_type = *(betype<uint64>*)data;
data += 6; // 48
be_type >>= 16;
be_type &= 0xFFFFFFFFFFFF;
switch (be_type.value())
{
case kExtensionNunchuck:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Nunchuck");
new_state.m_extension = NunchuckData{};
break;
case kExtensionClassic:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Classic");
new_state.m_extension = ClassicData{};
break;
case kExtensionClassicPro:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Classic Pro");
break;
case kExtensionGuitar:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Guitar");
break;
case kExtensionDrums:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Drums");
break;
case kExtensionBalanceBoard:
cemuLog_logDebug(LogType::Force,"Extension Type Received: Balance Board");
break;
case kExtensionMotionPlus:
cemuLog_logDebug(LogType::Force,"Extension Type Received: MotionPlus");
set_motion_plus(index, true);
new_state.m_motion_plus = MotionPlusData{};
break;
case kExtensionPartialyInserted:
cemuLog_logDebug(LogType::Force,"Extension only partially inserted");
new_state.m_extension = {};
request_status(index);
break;
default:
cemuLog_logDebug(LogType::Force,"Unknown extension: {:#x}", be_type.value());
new_state.m_extension = {};
break;
}
if (new_state.m_extension.index() != 0)
send_read_packet(index, kRegisterMemory, kRegisterExtensionCalibration, 0x10);
}
else if (address == (kRegisterExtensionCalibration & 0xFFFF))
{
cemu_assert(size == 0x10);
cemuLog_logDebug(LogType::Force,"Extension calibration received");
std::visit(
overloaded
{
[](auto)
{
},
[data](MotionPlusData& mp)
{
// TODO fix
},
[data](NunchuckData& nunchuck)
{
std::array<uint8, 14> zero{};
if (memcmp(zero.data(), data, zero.size()) == 0)
{
cemuLog_logDebug(LogType::Force,"Extension calibration data is zero");
return;
}
nunchuck.calibration.zero.x = (uint16)data[0] << 2;
nunchuck.calibration.zero.y = (uint16)data[1] << 2;
nunchuck.calibration.zero.z = (uint16)data[2] << 2;
// --XXYYZZ
nunchuck.calibration.zero.x |= (data[3] >> 4) & 0x3; // 5|4 -> 1|0
nunchuck.calibration.zero.y |= (data[3] >> 2) & 0x3; // 3|4 -> 1|0
nunchuck.calibration.zero.z |= data[3] & 0x3;
nunchuck.calibration.gravity.x = (uint16)data[4] << 2;;
nunchuck.calibration.gravity.y = (uint16)data[5] << 2;;
nunchuck.calibration.gravity.z = (uint16)data[6] << 2;;
// --XXYYZZ
nunchuck.calibration.gravity.x |= (data[7] >> 4) & 0x3; // 5|4 -> 1|0
nunchuck.calibration.gravity.y |= (data[7] >> 2) & 0x3; // 3|4 -> 1|0
nunchuck.calibration.gravity.z |= data[7] & 0x3;
nunchuck.calibration.max.x = data[8];
nunchuck.calibration.max.y = data[11];
nunchuck.calibration.min.x = data[9];
nunchuck.calibration.min.y = data[12];
nunchuck.calibration.center.x = data[10];
nunchuck.calibration.center.y = data[13];
}
}, new_state.m_extension);
}
else
{
cemuLog_logDebug(LogType::Force,"Unhandled read data received");
continue;
}
update_report = true;
}
break;
case kAcknowledge:
{
new_state.buttons = *(uint16*)data & (~0x60E0);
data += 2;
const auto report_id = *data++;
const auto error = *data++;
if (error)
cemuLog_logDebug(LogType::Force, "Error {:#x} from output report {:#x}", error, report_id);
break;
}
case kDataCore:
{
// 30 BB BB
new_state.buttons = *(uint16*)data & (~0x60E0);
data += 2;
break;
}
case kDataCoreAcc:
{
// 31 BB BB AA AA AA
new_state.buttons = *(uint16*)data & (~0x60E0);
parse_acceleration(new_state, data);
break;
}
case kDataCoreExt8:
{
// 32 BB BB EE EE EE EE EE EE EE EE
new_state.buttons = *(uint16*)data & (~0x60E0);
data += 2;
break;
}
case kDataCoreAccIR:
{
// 33 BB BB AA AA AA II II II II II II II II II II II II
new_state.buttons = *(uint16*)data & (~0x60E0);
parse_acceleration(new_state, data);
data += parse_ir(new_state, data);
break;
}
case kDataCoreExt19:
{
// 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
new_state.buttons = *(uint16*)data & (~0x60E0);
data += 2;
break;
}
case kDataCoreAccExt:
{
// 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
new_state.buttons = *(uint16*)data & (~0x60E0);
parse_acceleration(new_state, data);
break;
}
case kDataCoreIRExt:
{
// 36 BB BB II II II II II II II II II II EE EE EE EE EE EE EE EE EE
new_state.buttons = *(uint16*)data & (~0x60E0);
data += 2;
break;
}
case kDataCoreAccIRExt:
{
// 37 BB BB AA AA AA II II II II II II II II II II EE EE EE EE EE EE
new_state.buttons = *(uint16*)data & (~0x60E0);
parse_acceleration(new_state, data);
data += parse_ir(new_state, data); // 10
std::visit(
overloaded
{
[](auto)
{
},
[data](MotionPlusData& mp) mutable
{
glm::vec<3, uint16> raw;
raw.x = *data;
++data;
raw.y = *data;
++data;
raw.z = *data;
++data;
raw.x |= (uint16)*data << 6; // 7|2 -> 13|8
mp.slow_yaw = *data & 2;
mp.slow_pitch = *data & 1;
++data;
raw.y |= (uint16)*data << 6; // 7|2 -> 13|8
mp.slow_roll = *data & 2;
mp.extension_connected = *data & 1;
++data;
raw.z |= (uint16)*data << 6; // 7|2 -> 13|8
auto& calib = mp.calibration;
glm::vec3 orientation = raw;
/*orientation -= calib.zero;
Vector3<float> tmp = calib.gravity;
tmp -= calib.zero;
orientation /= tmp;*/
mp.orientation = orientation;
cemuLog_logDebug(LogType::Force,"MotionPlus: {:.2f}, {:.2f} {:.2f}", mp.orientation.x, mp.orientation.y, mp.orientation.z);
},
[data](NunchuckData& nunchuck) mutable
{
nunchuck.raw_axis.x = *data;
++data;
nunchuck.raw_axis.y = *data;
++data;
glm::vec<3, uint16> raw_acc;
raw_acc.x = (uint16)*data << 2;
++data;
raw_acc.y = (uint16)*data << 2;
++data;
raw_acc.z = (uint16)*data << 2;
++data;
nunchuck.z = (*data & 1) == 0;
nunchuck.c = (*data & 2) == 0;
raw_acc.x |= (*data >> 2) & 0x3; // 3|2 -> 1|0
raw_acc.y |= (*data >> 4) & 0x3; // 5|4 -> 1|0
raw_acc.z |= (*data >> 6) & 0x3; // 7|6 -> 1|0
auto& calib = nunchuck.calibration;
if (nunchuck.raw_axis.x < nunchuck.calibration.center.x) // [-1, 0]
nunchuck.axis.x = ((float)nunchuck.raw_axis.x - calib.min.x) / ((float)nunchuck.
calibration.center.x - calib.min.x + 0.012f) - 1.0f;
else // [0, 1]
nunchuck.axis.x = (float)(nunchuck.raw_axis.x - nunchuck.calibration.center.x) / (
nunchuck.calibration.max.x - nunchuck.calibration.center.x + 0.012f);
if (nunchuck.raw_axis.y <= nunchuck.calibration.center.y) // [-1, 0]
nunchuck.axis.y = ((float)nunchuck.raw_axis.y - calib.min.y) / ((float)nunchuck.
calibration.center.y - calib.min.y + 0.012f) - 1.0f;
else // [0, 1]
nunchuck.axis.y = (float)(nunchuck.raw_axis.y - nunchuck.calibration.center.y) / (
nunchuck.calibration.max.y - nunchuck.calibration.center.y);
glm::vec3 acceleration = raw_acc;
nunchuck.prev_acceleration = nunchuck.acceleration;
nunchuck.acceleration = acceleration - glm::vec3(calib.zero);
float acc[3]{ -nunchuck.acceleration.x, -nunchuck.acceleration.z, nunchuck.acceleration.y };
const auto grav = nunchuck.calibration.gravity - nunchuck.calibration.zero;
auto tacc = nunchuck.acceleration;
auto pacc = nunchuck.prev_acceleration;
if (grav != glm::vec<3, uint16>{})
{
acc[0] /= (float)grav.x;
acc[1] /= (float)grav.y;
acc[2] /= (float)grav.z;
tacc.x /= (float)grav.x;
pacc.x /= (float)grav.x;
tacc.y /= (float)grav.y;
pacc.y /= (float)grav.y;
tacc.z /= (float)grav.z;
pacc.z /= (float)grav.z;
}
float zero3[3]{};
float zero4[4]{};
nunchuck.motion_sample = MotionSample(
acc,
glm::length(tacc - pacc),
zero3,
zero3,
zero4
);
cemuLog_logDebug(LogType::Force,"Nunchuck: Z={}, C={} | {}, {} | {:.2f}, {:.2f}, {:.2f}",
nunchuck.z, nunchuck.c,
nunchuck.axis.x, nunchuck.axis.y,
RadToDeg(nunchuck.acceleration.x), RadToDeg(nunchuck.acceleration.y),
RadToDeg(nunchuck.acceleration.z));
},
[data](ClassicData& classic) mutable
{
classic.left_raw_axis.x = *data & 0x3F;
classic.right_raw_axis.x = (*data & 0xC0) >> 3; // 7|6 -> 4|3
++data;
classic.left_raw_axis.y = *data & 0x3F;
classic.right_raw_axis.x |= (*data & 0xC0) >> 5; // 7|6 -> 2|1
++data;
classic.right_raw_axis.y = *data & 0x1F;
classic.raw_trigger.x = (*data & 0x60) >> 2; // 6|5 -> 4|3
classic.right_raw_axis.x |= (*data & 0x80) >> 7; // 7 -> 0
++data;
classic.raw_trigger.x |= (*data & 0xE0) >> 5; // 7|5 -> 2|0
classic.raw_trigger.y = (*data & 0x1F);
++data;
classic.buttons = ~(*(uint16*)data);
data += 2;
classic.left_axis = classic.left_raw_axis;
classic.left_axis /= 63.0f;
classic.left_axis = classic.left_axis * 2.0f - 1.0f;
classic.right_axis = classic.right_raw_axis;
classic.right_axis /= 31.0f;
classic.right_axis = classic.right_axis * 2.0f - 1.0f;
classic.trigger = classic.raw_trigger;
classic.trigger /= 31.0f;
cemuLog_logDebug(LogType::Force,"Classic Controller: Buttons={:b} | {}, {} | {}, {} | {}, {}",
classic.buttons, classic.left_axis.x, classic.left_axis.y,
classic.right_axis.x, classic.right_axis.y, classic.trigger.x,
classic.trigger.y);
}
}, new_state.m_extension);
break;
}
case kDataExt:
{
// 3d EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
break;
}
default:
cemuLog_logDebug(LogType::Force,"unhandled input packet id {} for wiimote {}", id, index);
}
// update motion data
//const auto motionnow = std::chrono::high_resolution_clock::now();
//const auto delta_time = (float)std::chrono::duration_cast<std::chrono::milliseconds>(motionnow - new_state.m_last_motion_timestamp).count() / 1000.0f;
//new_state.m_last_motion_timestamp = motionnow;
float acc[3]{-new_state.m_acceleration.x, -new_state.m_acceleration.z, new_state.m_acceleration.y};
const auto grav = new_state.m_calib_acceleration.gravity - new_state.m_calib_acceleration.zero;
auto tacc = new_state.m_acceleration;
auto pacc = new_state.m_prev_acceleration;
if (grav != glm::vec<3, uint16>{})
{
acc[0] /= (float)grav.x;
acc[1] /= (float)grav.y;
acc[2] /= (float)grav.z;
tacc.x /= (float)grav.x;
pacc.x /= (float)grav.x;
tacc.y /= (float)grav.y;
pacc.y /= (float)grav.y;
tacc.z /= (float)grav.z;
pacc.z /= (float)grav.z;
}
float zero3[3]{};
float zero4[4]{};
new_state.motion_sample = MotionSample(
acc,
glm::length(tacc - pacc),
zero3,
zero3,
zero4
);
std::unique_lock data_lock(wiimote.mutex);
wiimote.state = new_state;
data_lock.unlock();
if (update_report)
update_report_type(index);
}
lock.unlock();
if (!receivedAnyPacket)
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
}
void WiimoteControllerProvider::parse_acceleration(WiimoteState& wiimote_state, const uint8*& data)
{
glm::vec<3, uint16> raw_acc;
// acc encoded in BB BB
raw_acc.x = (*data >> 5) & 3; // 6|5 -> 1|0
++data;
raw_acc.y = (*data >> 4) & 2; // 5 -> 1
raw_acc.z = (*data >> 5) & 2; // 6 -> 1
++data;
raw_acc.x |= (uint16)*data << 2;
++data;
raw_acc.y |= (uint16)*data << 2;
++data;
raw_acc.z |= (uint16)*data << 2;
++data;
wiimote_state.m_prev_acceleration = wiimote_state.m_acceleration;
glm::vec3 acceleration = raw_acc;
const auto& calib = wiimote_state.m_calib_acceleration;
wiimote_state.m_acceleration = acceleration - glm::vec3(calib.zero);
glm::vec3 tmp = calib.gravity;
tmp -= calib.zero;
acceleration = (wiimote_state.m_acceleration / tmp);
const float pi_2 = (float)std::numbers::pi / 2.0f;
wiimote_state.m_roll = std::atan2(acceleration.z, acceleration.x) - pi_2;
}
void WiimoteControllerProvider::rotate_ir(WiimoteState& wiimote_state)
{
const float rot = wiimote_state.m_roll;
if (rot == 0.0f)
return;
const float sin = std::sin(rot);
const float cos = std::cos(rot);
int i = -1;
for (auto& dot : wiimote_state.ir_camera.dots)
{
i++;
if (!dot.visible)
continue;
// move to center, rotate and move back
dot.pos -= 0.5f;
dot.pos.x = (dot.pos.x * cos) + (dot.pos.y * (-sin));
dot.pos.y = (dot.pos.x * sin) + (dot.pos.y * cos);
dot.pos += 0.5f;
}
}
void WiimoteControllerProvider::calculate_ir_position(WiimoteState& wiimote_state)
{
auto& ir = wiimote_state.ir_camera;
ir.m_prev_position = ir.position;
std::pair indices = ir.indices;
if (ir.middle.x != 0)
{
const float last_angle = std::atan(ir.middle.y / ir.middle.x);
float best_distance = std::numeric_limits<float>::max();
for (size_t i = 0; i < ir.dots.size(); ++i)
{
if (!ir.dots[i].visible)
continue;
for (size_t j = i + 1; j < ir.dots.size(); ++j)
{
if (!ir.dots[j].visible)
continue;
const auto mid = (ir.dots[i].pos + ir.dots[j].pos) / 2.0f;
if (mid.x == 0)
continue;
// check if angle is close enough to the last known one
float angle = std::atan(mid.y / mid.x);
if (std::abs(last_angle - angle) > DegToRad(10.0f))
continue;
// check if distance between points is similar to last known distance
const float distance = std::abs(ir.distance - glm::length(ir.dots[i].pos - ir.dots[j].pos));
if (distance > 0.1f && distance > best_distance)
continue;
// found a new pair
best_distance = distance;
indices = {(sint32)i, (sint32)j};
}
}
}
if (ir.dots[indices.first].visible && ir.dots[indices.second].visible)
{
ir.prev_dots[indices.first] = ir.dots[indices.first];
ir.prev_dots[indices.second] = ir.dots[indices.second];
ir.position = (ir.dots[indices.first].pos + ir.dots[indices.second].pos) / 2.0f;
ir.middle = ir.position;
ir.distance = glm::length(ir.dots[indices.first].pos - ir.dots[indices.second].pos);
ir.indices = indices;
ir.m_positionVisibility = PositionVisibility::FULL;
}
else if (ir.dots[indices.first].visible)
{
ir.position = ir.middle + (ir.dots[indices.first].pos - ir.prev_dots[indices.first].pos);
ir.m_positionVisibility = PositionVisibility::PARTIAL;
}
else if (ir.dots[indices.second].visible)
{
ir.position = ir.middle + (ir.dots[indices.second].pos - ir.prev_dots[indices.second].pos);
ir.m_positionVisibility = PositionVisibility::PARTIAL;
}
else {
ir.m_positionVisibility = PositionVisibility::NONE;
}
}
sint32 WiimoteControllerProvider::parse_ir(WiimoteState& wiimote_state, const uint8* data)
{
switch (wiimote_state.ir_camera.mode)
{
case kIRDisabled:
wiimote_state.ir_camera.dots = {};
return 0;
case kBasicIR:
{
const auto ir = (BasicIR*)data;
for (int i = 0; i < 2; ++i)
{
auto& dot1 = wiimote_state.ir_camera.dots[i * 2];
auto& dot2 = wiimote_state.ir_camera.dots[i * 2 + 1];
dot1.raw.x = ir[i].x1 | (ir[i].bits.x1 << 8); // 9|8
dot1.raw.y = ir[i].y1 | (ir[i].bits.y1 << 8);
dot1.size = 0;
dot2.raw.x = ir[i].x2 | (ir[i].bits.x2 << 8);
dot2.raw.y = ir[i].y2 | (ir[i].bits.y2 << 8);
dot2.size = 0;
dot1.visible = dot1.raw != glm::vec<2, uint16>(0x3ff, 0x3ff);
if (dot1.visible)
dot1.pos = glm::vec2(1.0f - dot1.raw.x / 1023.0f, (float)dot1.raw.y / 768.0f);
else
dot1.pos = {};
dot2.visible = dot2.raw != glm::vec<2, uint16>(0x3ff, 0x3ff);
if (dot2.visible)
dot2.pos = glm::vec2(1.0f - dot2.raw.x / 1023.0f, (float)dot2.raw.y / 768.0f);
else
dot2.pos = {};
}
rotate_ir(wiimote_state);
calculate_ir_position(wiimote_state);
return sizeof(BasicIR) * 2;
}
case kExtendedIR:
{
const auto ir = (ExtendedIR*)data;
for (int i = 0; i < 4; ++i)
{
auto& dot = wiimote_state.ir_camera.dots[i];
dot.raw.x = ir[i].x;
dot.raw.y = ir[i].y;
dot.raw.x |= (uint16)ir[i].bits.x << 8; // 9|8
dot.raw.y |= (uint16)ir[i].bits.y << 8; // 9|8
dot.size = ir[i].bits.size;
dot.visible = dot.raw != glm::vec<2, uint16>(0x3ff, 0x3ff);
if (dot.visible)
dot.pos = glm::vec2(1.0f - dot.raw.x / 1023.0f, (float)dot.raw.y / 768.0f);
else
dot.pos = {};
}
rotate_ir(wiimote_state);
calculate_ir_position(wiimote_state);
return sizeof(ExtendedIR) * 4;
}
default:
cemu_assert(false);
break;
}
return 0;
}
void WiimoteControllerProvider::request_extension(size_t index)
{
// send_write_packet(index, kRegisterMemory, kRegisterExtensionEncrypted, { 0x00 });
send_write_packet(index, kRegisterMemory, kRegisterExtension1, {0x55});
send_write_packet(index, kRegisterMemory, kRegisterExtension2, {0x00});
send_read_packet(index, kRegisterMemory, kRegisterExtensionType, 6);
}
void WiimoteControllerProvider::detect_motion_plus(size_t index)
{
send_read_packet(index, kRegisterMemory, kRegisterMotionPlusDetect, 6);
}
void WiimoteControllerProvider::set_motion_plus(size_t index, bool state)
{
if (state) {
send_write_packet(index, kRegisterMemory, kRegisterMotionPlusInit, { 0x55 });
send_write_packet(index, kRegisterMemory, kRegisterMotionPlusEnable, { 0x04 });
}
else
{
send_write_packet(index, kRegisterMemory, kRegisterExtension1, { 0x55 });
}
}
void WiimoteControllerProvider::writer_thread()
{
SetThreadName("Wiimote-writer");
while (m_running.load(std::memory_order_relaxed))
{
std::unique_lock writer_lock(m_writer_mutex);
while (m_write_queue.empty())
{
if (m_writer_cond.wait_for(writer_lock, std::chrono::milliseconds(250)) == std::cv_status::timeout)
{
if (!m_running.load(std::memory_order_relaxed))
return;
}
}
auto index = (size_t)-1;
std::vector<uint8> data;
std::shared_lock device_lock(m_device_mutex);
// get first packet of device which is ready to be sent
const auto now = std::chrono::high_resolution_clock::now();
for (auto it = m_write_queue.begin(); it != m_write_queue.end(); ++it)
{
if (it->first >= m_wiimotes.size())
continue;
const auto delay = m_wiimotes[it->first].data_delay.load(std::memory_order_relaxed);
if (now >= m_wiimotes[it->first].data_ts + std::chrono::milliseconds(delay))
{
index = it->first;
data = it->second;
m_write_queue.erase(it);
break;
}
}
writer_lock.unlock();
if (index != (size_t)-1 && !data.empty())
{
auto& wiimote = m_wiimotes[index];
if (!wiimote.device)
continue;
if (wiimote.rumble)
data[1] |= 1;
if (!wiimote.device->write_data(data))
{
wiimote.device.reset();
wiimote.rumble = false;
}
else
wiimote.data_ts = std::chrono::high_resolution_clock::now();
}
device_lock.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(1));
std::this_thread::yield();
}
}
void WiimoteControllerProvider::calibrate(size_t index)
{
send_read_packet(index, kEEPROMMemory, kRegisterCalibration, 8);
}
void WiimoteControllerProvider::update_report_type(size_t index)
{
std::shared_lock read_lock(m_wiimotes[index].mutex);
auto& state = m_wiimotes[index].state;
const bool extension = state.m_extension.index() != 0; // TODO || HasMotionPlus();
const bool ir = state.ir_camera.mode != kIRDisabled;
const bool motion = true; // UseMotion();
InputReportId report_type;
if (extension && ir && motion)
report_type = kDataCoreAccIRExt;
else if (extension && ir)
report_type = kDataCoreIRExt;
else if (extension && motion)
report_type = kDataCoreAccExt;
else if (ir && motion)
report_type = kDataCoreAccIR;
else if (extension)
report_type = kDataCoreExt19;
else if (ir)
report_type = kDataCoreAccIR;
else if (motion)
report_type = kDataCoreAcc;
else
report_type = kDataCore;
cemuLog_logDebug(LogType::Force,"Setting report type to {}", report_type);
send_packet(index, {kType, 0x04, report_type});
state.ir_camera.mode = set_ir_camera(index, true);
}
IRMode WiimoteControllerProvider::set_ir_camera(size_t index, bool state)
{
std::shared_lock read_lock(m_wiimotes[index].mutex);
auto& wiimote_state = m_wiimotes[index].state;
IRMode mode;
if (!state)
mode = kIRDisabled;
else
{
mode = wiimote_state.m_extension.index() == 0 ? kExtendedIR : kBasicIR;
}
if (wiimote_state.ir_camera.mode == mode)
return mode;
wiimote_state.ir_camera.mode = mode;
const uint8_t data = state ? 0x04 : 0x00;
send_packet(index, {kIR, data});
send_packet(index, {kIR2, data});
if (state)
{
send_write_packet(index, kRegisterMemory, kRegisterIR, {0x08});
send_write_packet(index, kRegisterMemory, kRegisterIRSensitivity1,
{0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0xaa, 0x00, 0x64});
send_write_packet(index, kRegisterMemory, kRegisterIRSensitivity2, {0x63, 0x03});
send_write_packet(index, kRegisterMemory, kRegisterIRMode, {(uint8)mode});
send_write_packet(index, kRegisterMemory, kRegisterIR, {0x08});
}
update_report_type(index);
return mode;
}
void WiimoteControllerProvider::send_packet(size_t index, std::vector<uint8> data)
{
cemu_assert(data.size() > 1);
std::shared_lock device_lock(m_device_mutex);
if (index >= m_wiimotes.size())
return;
device_lock.unlock();
std::unique_lock lock(m_writer_mutex);
m_write_queue.emplace_back(index, data);
m_writer_cond.notify_one();
}
void WiimoteControllerProvider::send_read_packet(size_t index, MemoryType type, RegisterAddress address, uint16 size)
{
std::vector<uint8> data(7);
data[0] = kReadMemory;
data[1] = type;
*(betype<uint32>*)(data.data() + 2) = (address & 0xFFFFFF) << 8; // only uint24
*(betype<uint16>*)(data.data() + 2 + 3) = size;
send_packet(index, std::move(data));
}
void WiimoteControllerProvider::send_write_packet(size_t index, MemoryType type, RegisterAddress address,
const std::vector<uint8>& data)
{
cemu_assert(data.size() <= 16);
std::vector<uint8> packet(6 + 16);
packet[0] = kWriteMemory;
packet[1] = type;
*(betype<uint32>*)(packet.data() + 2) = (address & 0xFFFFFF) << 8; // only uint24
*(packet.data() + 2 + 3) = (uint8)data.size();
std::copy(data.begin(), data.end(), packet.data() + 2 + 3 + 1);
send_packet(index, std::move(packet));
}
Reference in a new issue View git blame Copy permalink