From 5361e664595bc32e8e593e8d0e588d1768bc4f73 Mon Sep 17 00:00:00 2001 From: Jordan Woyak Date: Sun, 19 Jan 2020 08:30:07 -0600 Subject: [PATCH 1/2] HW/WiimoteEmu: Camera logic cleanups. --- Source/Core/Core/HW/WiimoteEmu/Camera.cpp | 230 ++++++++++------------ Source/Core/Core/HW/WiimoteEmu/Camera.h | 48 ++++- 2 files changed, 142 insertions(+), 136 deletions(-) diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp index 32cc1752d1..05e28c7930 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp @@ -19,14 +19,14 @@ namespace WiimoteEmu { void CameraLogic::Reset() { - reg_data = {}; + m_reg_data = {}; m_is_enabled = false; } void CameraLogic::DoState(PointerWrap& p) { - p.Do(reg_data); + p.Do(m_reg_data); // FYI: m_is_enabled is handled elsewhere. } @@ -39,7 +39,7 @@ int CameraLogic::BusRead(u8 slave_addr, u8 addr, int count, u8* data_out) if (!m_is_enabled) return 0; - return RawRead(®_data, addr, count, data_out); + return RawRead(&m_reg_data, addr, count, data_out); } int CameraLogic::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) @@ -50,11 +50,26 @@ int CameraLogic::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) if (!m_is_enabled) return 0; - return RawWrite(®_data, addr, count, data_in); + return RawWrite(&m_reg_data, addr, count, data_in); } void CameraLogic::Update(const Common::Matrix44& transform) { + // IR data is read from offset 0x37 on real hardware. + auto& data = m_reg_data.camera_data; + data.fill(0xff); + + constexpr u8 OBJECT_TRACKING_ENABLE = 0x08; + + // If Address 0x30 is not 0x08 the camera will return 0xFFs. + // The Wii seems to write 0x01 here before changing modes/sensitivities. + if (m_reg_data.enable_object_tracking != OBJECT_TRACKING_ENABLE) + return; + + // If the sensor bar is off the camera will see no LEDs and return 0xFFs. + if (!IOS::g_gpio_out[IOS::GPIO::SENSOR_BAR]) + return; + using Common::Matrix33; using Common::Matrix44; using Common::Vec3; @@ -86,141 +101,98 @@ void CameraLogic::Update(const Common::Matrix44& transform) struct CameraPoint { - u16 x; - u16 y; + IRBasic::IRObject position; u8 size; }; - // 0xFFFFs are interpreted as "not visible". - constexpr CameraPoint INVISIBLE_POINT{0xffff, 0xffff, 0xff}; - std::array camera_points; - if (IOS::g_gpio_out[IOS::GPIO::SENSOR_BAR]) - { - std::transform(leds.begin(), leds.end(), camera_points.begin(), [&](const Vec3& v) { - const auto point = camera_view * Vec4(v, 1.0); + std::transform(leds.begin(), leds.end(), camera_points.begin(), [&](const Vec3& v) { + const auto point = camera_view * Vec4(v, 1.0); - if (point.z > 0) - { - // FYI: Casting down vs. rounding seems to produce more symmetrical output. - const auto x = s32((1 - point.x / point.w) * CAMERA_RES_X / 2); - const auto y = s32((1 - point.y / point.w) * CAMERA_RES_Y / 2); - - const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2); - - if (x >= 0 && y >= 0 && x < CAMERA_RES_X && y < CAMERA_RES_Y) - return CameraPoint{u16(x), u16(y), u8(point_size)}; - } - - return INVISIBLE_POINT; - }); - } - else - { - // Sensor bar is off - camera_points.fill(INVISIBLE_POINT); - } - - // IR data is read from offset 0x37 on real hardware - auto& data = reg_data.camera_data; - // A maximum of 36 bytes: - std::fill(std::begin(data), std::end(data), 0xff); - - // Fill report with valid data when full handshake was done - // TODO: kill magic number: - if (reg_data.data[0x30]) - { - switch (reg_data.mode) + // Check if LED is behind camera. + if (point.z > 0) { - case IR_MODE_BASIC: - for (std::size_t i = 0; i != camera_points.size() / 2; ++i) - { - IRBasic irdata = {}; + // FYI: Casting down vs. rounding seems to produce more symmetrical output. + const auto x = s32((1 - point.x / point.w) * CAMERA_RES_X / 2); + const auto y = s32((1 - point.y / point.w) * CAMERA_RES_Y / 2); - const auto& p1 = camera_points[i * 2]; - irdata.x1 = p1.x; - irdata.x1hi = p1.x >> 8; - irdata.y1 = p1.y; - irdata.y1hi = p1.y >> 8; + const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2); - const auto& p2 = camera_points[i * 2 + 1]; - irdata.x2 = p2.x; - irdata.x2hi = p2.x >> 8; - irdata.y2 = p2.y; - irdata.y2hi = p2.y >> 8; - - Common::BitCastPtr(data + i * sizeof(IRBasic)) = irdata; - } - break; - case IR_MODE_EXTENDED: - for (std::size_t i = 0; i != camera_points.size(); ++i) - { - const auto& p = camera_points[i]; - if (p.x < CAMERA_RES_X) - { - IRExtended irdata = {}; - - // TODO: Move this logic into IRExtended class? - irdata.x = p.x; - irdata.xhi = p.x >> 8; - - irdata.y = p.y; - irdata.yhi = p.y >> 8; - - irdata.size = p.size; - - Common::BitCastPtr(data + i * sizeof(IRExtended)) = irdata; - } - } - break; - case IR_MODE_FULL: - for (std::size_t i = 0; i != camera_points.size(); ++i) - { - const auto& p = camera_points[i]; - if (p.x < CAMERA_RES_X) - { - IRFull irdata = {}; - - irdata.x = p.x; - irdata.xhi = p.x >> 8; - - irdata.y = p.y; - irdata.yhi = p.y >> 8; - - irdata.size = p.size; - - // TODO: does size need to be scaled up? - // E.g. does size 15 cover the entire sensor range? - - irdata.xmin = std::max(p.x - p.size, 0); - irdata.ymin = std::max(p.y - p.size, 0); - irdata.xmax = std::min(p.x + p.size, CAMERA_RES_X); - irdata.ymax = std::min(p.y + p.size, CAMERA_RES_Y); - - // TODO: Is this maybe MSbs of the "intensity" value? - irdata.zero = 0; - - constexpr int SUBPIXEL_RESOLUTION = 8; - constexpr long MAX_INTENSITY = 0xff; - - // This is apparently the number of pixels the point takes up at 128x96 resolution. - // We simulate a circle that shrinks at sensor edges. - const auto intensity = - std::lround((irdata.xmax - irdata.xmin) * (irdata.ymax - irdata.ymin) / - SUBPIXEL_RESOLUTION / SUBPIXEL_RESOLUTION * MathUtil::TAU / 8); - - irdata.intensity = u8(std::min(MAX_INTENSITY, intensity)); - - Common::BitCastPtr(data + i * sizeof(IRFull)) = irdata; - } - } - break; - default: - // This seems to be fairly common, 0xff data is sent in this case: - // WARN_LOG(WIIMOTE, "Game is requesting IR data before setting IR mode."); - break; + if (x >= 0 && y >= 0 && x < CAMERA_RES_X && y < CAMERA_RES_Y) + return CameraPoint{{u16(x), u16(y)}, u8(point_size)}; } + + // 0xFFFFs are interpreted as "not visible". + return CameraPoint{{0xffff, 0xffff}, 0xff}; + }); + + switch (m_reg_data.mode) + { + case IR_MODE_BASIC: + for (std::size_t i = 0; i != camera_points.size() / 2; ++i) + { + IRBasic irdata = {}; + + irdata.SetObject1(camera_points[i * 2].position); + irdata.SetObject2(camera_points[i * 2 + 1].position); + + Common::BitCastPtr(&data[i * sizeof(IRBasic)]) = irdata; + } + break; + case IR_MODE_EXTENDED: + for (std::size_t i = 0; i != camera_points.size(); ++i) + { + const auto& p = camera_points[i]; + if (p.position.x < CAMERA_RES_X) + { + IRExtended irdata = {}; + + irdata.SetPosition(p.position); + irdata.size = p.size; + + Common::BitCastPtr(&data[i * sizeof(IRExtended)]) = irdata; + } + } + break; + case IR_MODE_FULL: + for (std::size_t i = 0; i != camera_points.size(); ++i) + { + const auto& p = camera_points[i]; + if (p.position.x < CAMERA_RES_X) + { + IRFull irdata = {}; + + irdata.SetPosition(p.position); + irdata.size = p.size; + + // TODO: does size need to be scaled up? + // E.g. does size 15 cover the entire sensor range? + + irdata.xmin = std::max(p.position.x - p.size, 0); + irdata.ymin = std::max(p.position.y - p.size, 0); + irdata.xmax = std::min(p.position.x + p.size, CAMERA_RES_X); + irdata.ymax = std::min(p.position.y + p.size, CAMERA_RES_Y); + + constexpr int SUBPIXEL_RESOLUTION = 8; + constexpr long MAX_INTENSITY = 0xff; + + // This is apparently the number of pixels the point takes up at 128x96 resolution. + // We simulate a circle that shrinks at sensor edges. + const auto intensity = + std::lround((irdata.xmax - irdata.xmin) * (irdata.ymax - irdata.ymin) / + SUBPIXEL_RESOLUTION / SUBPIXEL_RESOLUTION * MathUtil::TAU / 8); + + irdata.intensity = u8(std::min(MAX_INTENSITY, intensity)); + + Common::BitCastPtr(&data[i * sizeof(IRFull)]) = irdata; + } + } + break; + default: + // This seems to be fairly common, 0xff data is sent in this case: + // WARN_LOG(WIIMOTE, "Game is requesting IR data before setting IR mode."); + break; } } diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.h b/Source/Core/Core/HW/WiimoteEmu/Camera.h index 547610bd9b..4a98d33325 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Camera.h +++ b/Source/Core/Core/HW/WiimoteEmu/Camera.h @@ -33,6 +33,21 @@ struct IRBasic auto GetObject1() const { return IRObject(x1hi << 8 | x1, y1hi << 8 | y1); } auto GetObject2() const { return IRObject(x2hi << 8 | x2, y2hi << 8 | y2); } + + void SetObject1(const IRObject& obj) + { + x1 = obj.x; + x1hi = obj.x >> 8; + y1 = obj.y; + y1hi = obj.y >> 8; + } + void SetObject2(const IRObject& obj) + { + x2 = obj.x; + x2hi = obj.x >> 8; + y2 = obj.y; + y2hi = obj.y >> 8; + } }; static_assert(sizeof(IRBasic) == 5, "Wrong size"); @@ -44,6 +59,15 @@ struct IRExtended u8 size : 4; u8 xhi : 2; u8 yhi : 2; + + auto GetPosition() const { return IRBasic::IRObject(xhi << 8 | x, yhi << 8 | y); } + void SetPosition(const IRBasic::IRObject& obj) + { + x = obj.x; + xhi = obj.x >> 8; + y = obj.y; + yhi = obj.y >> 8; + } }; static_assert(sizeof(IRExtended) == 3, "Wrong size"); @@ -96,15 +120,25 @@ private: struct Register { // Contains sensitivity and other unknown data - // TODO: Do the IR and Camera enabling reports write to the i2c bus? // TODO: Does disabling the camera peripheral reset the mode or sensitivity? - // TODO: Break out this "data" array into some known members - u8 data[0x33]; + std::array sensitivity_block1; + std::array unk_0x09; + + // addr: 0x1a + std::array sensitivity_block2; + std::array unk_0x1c; + + // addr: 0x30 + u8 enable_object_tracking; + std::array unk_0x31; + + // addr: 0x33 u8 mode; - u8 unk[3]; + std::array unk_0x34; + // addr: 0x37 - u8 camera_data[CAMERA_DATA_BYTES]; - u8 unk2[165]; + std::array camera_data; + std::array unk_0x5b; }; #pragma pack(pop) @@ -118,7 +152,7 @@ private: int BusRead(u8 slave_addr, u8 addr, int count, u8* data_out) override; int BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) override; - Register reg_data; + Register m_reg_data; // When disabled the camera does not respond on the bus. // Change is triggered by wiimote report 0x13. From ef777c4186778272a6afc0d1c8909d7f75eaa703 Mon Sep 17 00:00:00 2001 From: Jordan Woyak Date: Mon, 24 Feb 2020 19:04:48 -0600 Subject: [PATCH 2/2] HW/WiimoteEmu: Fill IR data with 0xFF on failed bus read. --- Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp index a9d564d116..cd05f70426 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -516,8 +516,15 @@ void Wiimote::SendDataReport() const u8 camera_data_offset = CameraLogic::REPORT_DATA_OFFSET + rpt_builder.GetIRDataFormatOffset(); - m_i2c_bus.BusRead(CameraLogic::I2C_ADDR, camera_data_offset, rpt_builder.GetIRDataSize(), - rpt_builder.GetIRDataPtr()); + u8* ir_data = rpt_builder.GetIRDataPtr(); + const u8 ir_size = rpt_builder.GetIRDataSize(); + + if (ir_size != m_i2c_bus.BusRead(CameraLogic::I2C_ADDR, camera_data_offset, ir_size, ir_data)) + { + // This happens when IR reporting is enabled but the camera hardware is disabled. + // It commonly occurs when changing IR sensitivity. + std::fill_n(ir_data, ir_size, u8(0xff)); + } } // Extension port: @@ -541,7 +548,7 @@ void Wiimote::SendDataReport() ExtensionPort::REPORT_I2C_ADDR, ext_size, ext_data)) { // Real wiimote seems to fill with 0xff on failed bus read - std::fill_n(ext_data, ext_size, 0xff); + std::fill_n(ext_data, ext_size, u8(0xff)); } }