diff --git a/rootfs/usr/share/inputplumber/devices/50-legion_go.yaml b/rootfs/usr/share/inputplumber/devices/50-legion_go.yaml index fd747b25..9d7bcf60 100644 --- a/rootfs/usr/share/inputplumber/devices/50-legion_go.yaml +++ b/rootfs/usr/share/inputplumber/devices/50-legion_go.yaml @@ -156,6 +156,15 @@ source_devices: product_id: 0x61ee interface_num: 2 + # IMU + - group: imu + iio: + name: accel_3d + mount_matrix: + x: [0, 1, 0] + y: [1, 0, 0] + z: [0, 0, 1] + # IMU - group: imu iio: diff --git a/src/drivers/dualsense/driver.rs b/src/drivers/dualsense/driver.rs index 06798d00..b7b51cfa 100644 --- a/src/drivers/dualsense/driver.rs +++ b/src/drivers/dualsense/driver.rs @@ -9,49 +9,11 @@ use crate::drivers::dualsense::{ }; use super::{ - event::{AccelerometerEvent, AccelerometerInput, AxisEvent, AxisInput, Event, TouchAxisInput}, + event::{AxisEvent, AxisInput, Event, InertialInput, IntertialEvent, TouchAxisInput}, hid_report::{PackedInputDataReport, SetStatePackedOutputData, UsbPackedOutputReport}, + DS5_ACCEL_TO_SI, DS5_GYRO_TO_RADS, DS5_VID, INPUT_REPORT_BT_SIZE, PIDS, }; -// Source: https://github.com/torvalds/linux/blob/master/drivers/hid/hid-playstation.c -pub const DS5_EDGE_NAME: &str = "Sony Interactive Entertainment DualSense Edge Wireless Controller"; -pub const DS5_EDGE_VERSION: u16 = 256; -pub const DS5_EDGE_VID: u16 = 0x054c; -pub const DS5_EDGE_PID: u16 = 0x0df2; - -pub const DS5_NAME: &str = "Sony Interactive Entertainment DualSense Wireless Controller"; -pub const DS5_VERSION: u16 = 0x8111; -pub const DS5_VID: u16 = 0x054c; -pub const DS5_PID: u16 = 0x0ce6; - -pub const PIDS: [u16; 2] = [DS5_EDGE_PID, DS5_PID]; - -pub const FEATURE_REPORT_PAIRING_INFO: u8 = 0x09; -pub const FEATURE_REPORT_FIRMWARE_INFO: u8 = 0x20; -pub const FEATURE_REPORT_CALIBRATION: u8 = 0x05; - -pub const INPUT_REPORT_USB: u8 = 0x01; -pub const INPUT_REPORT_USB_SIZE: usize = 64; -pub const INPUT_REPORT_BT: u8 = 0x31; -pub const INPUT_REPORT_BT_SIZE: usize = 78; -pub const OUTPUT_REPORT_USB: u8 = 0x02; -pub const OUTPUT_REPORT_USB_SIZE: usize = 63; -pub const OUTPUT_REPORT_USB_SHORT_SIZE: usize = 48; -pub const OUTPUT_REPORT_BT: u8 = 0x31; -pub const OUTPUT_REPORT_BT_SIZE: usize = 78; - -// Input report axis ranges -pub const STICK_X_MIN: f64 = u8::MIN as f64; -pub const STICK_X_MAX: f64 = u8::MAX as f64; -pub const STICK_Y_MIN: f64 = u8::MIN as f64; -pub const STICK_Y_MAX: f64 = u8::MAX as f64; -pub const TRIGGER_MAX: f64 = u8::MAX as f64; - -// DualSense hardware limits -pub const DS5_ACC_RES_PER_G: u32 = 8192; -pub const DS5_TOUCHPAD_WIDTH: f64 = 1919.0; -pub const DS5_TOUCHPAD_HEIGHT: f64 = 1079.0; - /// PS5 Dualsense controller driver for reading gamepad input pub struct Driver { state: Option, @@ -420,18 +382,18 @@ impl Driver { } // Accelerometer events - events.push(Event::Accelerometer(AccelerometerEvent::Accelerometer( - AccelerometerInput { - x: state.accel_x.to_primitive(), - y: state.accel_y.to_primitive(), - z: state.accel_z.to_primitive(), + events.push(Event::Accelerometer(IntertialEvent::Accelerometer( + InertialInput { + x: state.accel_x.to_primitive() as f64 * DS5_ACCEL_TO_SI, + y: state.accel_y.to_primitive() as f64 * DS5_ACCEL_TO_SI, + z: state.accel_z.to_primitive() as f64 * DS5_ACCEL_TO_SI, }, ))); - events.push(Event::Accelerometer(AccelerometerEvent::Gyro( - AccelerometerInput { - x: state.pitch.to_primitive(), - y: state.yaw.to_primitive(), - z: state.roll.to_primitive(), + events.push(Event::Accelerometer(IntertialEvent::Gyroscope( + InertialInput { + x: state.pitch.to_primitive() as f64 * DS5_GYRO_TO_RADS, + y: state.yaw.to_primitive() as f64 * DS5_GYRO_TO_RADS, + z: state.roll.to_primitive() as f64 * DS5_GYRO_TO_RADS, }, ))); diff --git a/src/drivers/dualsense/event.rs b/src/drivers/dualsense/event.rs index 98ff1ec9..f1a8d67e 100644 --- a/src/drivers/dualsense/event.rs +++ b/src/drivers/dualsense/event.rs @@ -2,7 +2,7 @@ #[derive(Clone, Debug)] pub enum Event { Button(ButtonEvent), - Accelerometer(AccelerometerEvent), + Accelerometer(IntertialEvent), Axis(AxisEvent), Trigger(TriggerEvent), } @@ -78,18 +78,18 @@ pub enum TriggerEvent { R2(TriggerInput), } -/// AccelerometerInput represents the state of the accelerometer (x, y, z) values +/// InertialInput represents the state of the IMU (x, y, z) values #[derive(Clone, Debug)] -pub struct AccelerometerInput { - pub x: i16, - pub y: i16, - pub z: i16, +pub struct InertialInput { + pub x: f64, + pub y: f64, + pub z: f64, } /// AccelerometerEvent has data from the accelerometer #[derive(Clone, Debug)] -pub enum AccelerometerEvent { - Accelerometer(AccelerometerInput), +pub enum IntertialEvent { + Accelerometer(InertialInput), /// Pitch, yaw, roll - Gyro(AccelerometerInput), + Gyroscope(InertialInput), } diff --git a/src/drivers/dualsense/hid_report.rs b/src/drivers/dualsense/hid_report.rs index 0faddf26..5ca96d36 100644 --- a/src/drivers/dualsense/hid_report.rs +++ b/src/drivers/dualsense/hid_report.rs @@ -5,7 +5,7 @@ use std::{error::Error, fmt::Display}; use packed_struct::prelude::*; -use super::driver::*; +use super::{INPUT_REPORT_BT, INPUT_REPORT_BT_SIZE, INPUT_REPORT_USB, INPUT_REPORT_USB_SIZE}; /// DualSense input report for USB and Bluetooth #[derive(Debug, Copy, Clone)] diff --git a/src/drivers/dualsense/mod.rs b/src/drivers/dualsense/mod.rs index 64242bb3..f81eca28 100644 --- a/src/drivers/dualsense/mod.rs +++ b/src/drivers/dualsense/mod.rs @@ -4,3 +4,45 @@ pub mod hid_report; #[cfg(test)] mod hid_report_test; pub mod report_descriptor; + +// Source: https://github.com/torvalds/linux/blob/master/drivers/hid/hid-playstation.c +pub const DS5_EDGE_NAME: &str = "Sony Interactive Entertainment DualSense Edge Wireless Controller"; +pub const DS5_EDGE_VERSION: u16 = 256; +pub const DS5_EDGE_VID: u16 = 0x054c; +pub const DS5_EDGE_PID: u16 = 0x0df2; + +pub const DS5_NAME: &str = "Sony Interactive Entertainment DualSense Wireless Controller"; +pub const DS5_VERSION: u16 = 0x8111; +pub const DS5_VID: u16 = 0x054c; +pub const DS5_PID: u16 = 0x0ce6; + +pub const PIDS: [u16; 2] = [DS5_EDGE_PID, DS5_PID]; + +pub const FEATURE_REPORT_PAIRING_INFO: u8 = 0x09; +pub const FEATURE_REPORT_FIRMWARE_INFO: u8 = 0x20; +pub const FEATURE_REPORT_CALIBRATION: u8 = 0x05; + +pub const INPUT_REPORT_USB: u8 = 0x01; +pub const INPUT_REPORT_USB_SIZE: usize = 64; +pub const INPUT_REPORT_BT: u8 = 0x31; +pub const INPUT_REPORT_BT_SIZE: usize = 78; +pub const OUTPUT_REPORT_USB: u8 = 0x02; +pub const OUTPUT_REPORT_USB_SIZE: usize = 63; +pub const OUTPUT_REPORT_USB_SHORT_SIZE: usize = 48; +pub const OUTPUT_REPORT_BT: u8 = 0x31; +pub const OUTPUT_REPORT_BT_SIZE: usize = 78; + +// Input report axis ranges +pub const STICK_X_MIN: f64 = u8::MIN as f64; +pub const STICK_X_MAX: f64 = u8::MAX as f64; +pub const STICK_Y_MIN: f64 = u8::MIN as f64; +pub const STICK_Y_MAX: f64 = u8::MAX as f64; +pub const TRIGGER_MAX: f64 = u8::MAX as f64; + +// DualSense hardware limits +pub const DS5_ACCEL_TO_SI: f64 = 0.00119710083; +pub const DS5_SI_TO_ACCEL: f64 = 0.101971621; +pub const DS5_GYRO_TO_RADS: f64 = 0.00001706026; +pub const DS5_RADS_TO_GYRO: f64 = 57.29577951; +pub const DS5_TOUCHPAD_WIDTH: f64 = 1919.0; +pub const DS5_TOUCHPAD_HEIGHT: f64 = 1079.0; diff --git a/src/drivers/flydigi_vader_4_pro/driver.rs b/src/drivers/flydigi_vader_4_pro/driver.rs index 1d411b55..238e798a 100644 --- a/src/drivers/flydigi_vader_4_pro/driver.rs +++ b/src/drivers/flydigi_vader_4_pro/driver.rs @@ -284,16 +284,16 @@ impl Driver { // Accelerometer events events.push(Event::Inertia(InertialEvent::Accelerometer( InertialInput { - x: -state.accel_x.to_primitive(), - y: state.accel_y.to_primitive(), - z: state.accel_z.to_primitive(), + x: -state.accel_x.to_primitive() as f64, + y: state.accel_y.to_primitive() as f64, + z: state.accel_z.to_primitive() as f64, }, ))); // Gyro events. They need to be rotated in order for them to be read properly - events.push(Event::Inertia(InertialEvent::Gyro(InertialInput { - x: -(state.gyro_x.to_primitive() as i32 * 1143239 / i16::MAX as i32) as i16, - y: -(state.get_y() as i32 * 1143239 / i16::MAX as i32) as i16, - z: -(state.gyro_z.to_primitive() as i32 * 17873 / i16::MAX as i32) as i16, + events.push(Event::Inertia(InertialEvent::Gyroscope(InertialInput { + x: -(state.gyro_x.to_primitive() as f64 * 1143239.0 / i16::MAX as f64), + y: -(state.get_y() as f64 * 1143239.0 / i16::MAX as f64), + z: -(state.gyro_z.to_primitive() as f64 * 17873.0 / i16::MAX as f64), }))); events } diff --git a/src/drivers/flydigi_vader_4_pro/event.rs b/src/drivers/flydigi_vader_4_pro/event.rs index 3ac2f5d8..da4a532d 100644 --- a/src/drivers/flydigi_vader_4_pro/event.rs +++ b/src/drivers/flydigi_vader_4_pro/event.rs @@ -69,16 +69,16 @@ pub enum ButtonEvent { /// [InertialInput] represents the state of the IMU (x, y, z) values #[derive(Clone, Debug)] pub struct InertialInput { - pub x: i16, - pub y: i16, - pub z: i16, + pub x: f64, + pub y: f64, + pub z: f64, } /// [InertialEvent] has data from the IMU #[derive(Clone, Debug)] pub enum InertialEvent { Accelerometer(InertialInput), - Gyro(InertialInput), + Gyroscope(InertialInput), } /// [JoystickInput] is a double (x, y) axis diff --git a/src/drivers/horipad_steam/driver.rs b/src/drivers/horipad_steam/driver.rs index 42a5c117..3d58b191 100644 --- a/src/drivers/horipad_steam/driver.rs +++ b/src/drivers/horipad_steam/driver.rs @@ -3,7 +3,10 @@ use std::{error::Error, ffi::CString}; use hidapi::HidDevice; use packed_struct::{types::SizedInteger, PackedStruct}; -use crate::{drivers::horipad_steam::hid_report::Direction, udev::device::UdevDevice}; +use crate::{ + drivers::horipad_steam::{hid_report::Direction, HORIPAD_ACCEL_TO_SI, HORIPAD_GYRO_TO_RADS}, + udev::device::UdevDevice, +}; use super::{ event::{ @@ -11,25 +14,9 @@ use super::{ JoystickInput, TriggerEvent, TriggerInput, }, hid_report::PackedInputDataReport, + HID_TIMEOUT, PACKET_SIZE, PIDS, REPORT_ID, VID, }; -// Report ID -pub const REPORT_ID: u8 = 0x07; - -// Input report size -const PACKET_SIZE: usize = 287; - -// HID buffer read timeout -const HID_TIMEOUT: i32 = 10; - -// Input report axis ranges -pub const JOY_AXIS_MAX: f64 = 255.0; -pub const JOY_AXIS_MIN: f64 = 0.0; -pub const TRIGGER_AXIS_MAX: f64 = 255.0; - -pub const VID: u16 = 0x0F0D; -pub const PIDS: [u16; 2] = [0x0196, 0x01AB]; - #[derive(Debug, Clone, Default)] struct DPadState { up: bool, @@ -289,15 +276,15 @@ impl Driver { // Accelerometer events events.push(Event::Inertia(InertialEvent::Accelerometer( InertialInput { - x: -state.accel_x.to_primitive(), - y: state.accel_y.to_primitive(), - z: -state.accel_z.to_primitive(), + x: -state.accel_x.to_primitive() as f64 * HORIPAD_ACCEL_TO_SI, + y: state.accel_y.to_primitive() as f64 * HORIPAD_ACCEL_TO_SI, + z: -state.accel_z.to_primitive() as f64 * HORIPAD_ACCEL_TO_SI, }, ))); - events.push(Event::Inertia(InertialEvent::Gyro(InertialInput { - x: -state.pitch.to_primitive(), - y: state.yaw.to_primitive(), - z: -state.roll.to_primitive(), + events.push(Event::Inertia(InertialEvent::Gyroscope(InertialInput { + x: -state.pitch.to_primitive() as f64 * HORIPAD_GYRO_TO_RADS, + y: state.yaw.to_primitive() as f64 * HORIPAD_GYRO_TO_RADS, + z: -state.roll.to_primitive() as f64 * HORIPAD_GYRO_TO_RADS, }))); log::trace!("Got events: {events:?}"); diff --git a/src/drivers/horipad_steam/event.rs b/src/drivers/horipad_steam/event.rs index c61ccbdb..3ee157c1 100644 --- a/src/drivers/horipad_steam/event.rs +++ b/src/drivers/horipad_steam/event.rs @@ -69,16 +69,16 @@ pub enum ButtonEvent { /// [InertialInput] represents the state of the IMU (x, y, z) values #[derive(Clone, Debug)] pub struct InertialInput { - pub x: i16, - pub y: i16, - pub z: i16, + pub x: f64, + pub y: f64, + pub z: f64, } /// [InertialEvent] has data from the IMU #[derive(Clone, Debug)] pub enum InertialEvent { Accelerometer(InertialInput), - Gyro(InertialInput), + Gyroscope(InertialInput), } /// [JoystickInput] is a double (x, y) axis diff --git a/src/drivers/horipad_steam/hid_report.rs b/src/drivers/horipad_steam/hid_report.rs index 58588e55..d89288ff 100644 --- a/src/drivers/horipad_steam/hid_report.rs +++ b/src/drivers/horipad_steam/hid_report.rs @@ -1,6 +1,6 @@ use packed_struct::prelude::*; -use super::driver::REPORT_ID; +use super::REPORT_ID; #[derive(PrimitiveEnum_u8, Clone, Copy, PartialEq, Debug, Default)] pub enum Direction { diff --git a/src/drivers/horipad_steam/mod.rs b/src/drivers/horipad_steam/mod.rs index 27d8325d..fdbd92c8 100644 --- a/src/drivers/horipad_steam/mod.rs +++ b/src/drivers/horipad_steam/mod.rs @@ -4,3 +4,25 @@ pub mod hid_report; #[cfg(test)] pub mod hid_report_test; pub mod report_descriptor; + +// Report ID +pub const REPORT_ID: u8 = 0x07; + +// Input report size +pub const PACKET_SIZE: usize = 287; + +// HID buffer read timeout +pub const HID_TIMEOUT: i32 = 10; + +// Input report axis ranges +pub const JOY_AXIS_MAX: f64 = 255.0; +pub const JOY_AXIS_MIN: f64 = 0.0; +pub const TRIGGER_AXIS_MAX: f64 = 255.0; + +pub const VID: u16 = 0x0F0D; +pub const PIDS: [u16; 2] = [0x0196, 0x01AB]; + +pub const HORIPAD_ACCEL_TO_SI: f64 = 0.00059855041; +pub const HORIPAD_SI_TO_ACCEL: f64 = 0.101971621; +pub const HORIPAD_GYRO_TO_RADS: f64 = 0.00106422393; +pub const HORIPAD_RADS_TO_GYRO: f64 = 57.29577951; diff --git a/src/drivers/iio_imu/bmi_driver.rs b/src/drivers/iio_imu/bmi_driver.rs new file mode 100644 index 00000000..28776fb3 --- /dev/null +++ b/src/drivers/iio_imu/bmi_driver.rs @@ -0,0 +1,681 @@ +use core::{option::Option::None, time::Duration}; +use std::{ + collections::{HashMap, HashSet}, + error::Error, + fs::{self}, + thread, +}; + +use industrial_io::{Buffer, Channel, ChannelType, Device, Direction}; + +use crate::input::capability::{Capability, Source}; + +use super::{ + event::{AxisData, Event}, + info::{Axis, AxisInfo, ImuGroup, MountMatrix}, +}; + +const DEFAULT_SAMPLE_RATE: f64 = 200.0; + +/// Driver for reading IIO IMU data +pub struct Driver { + _device: Device, // must outlive Channel raw pointers + buffer: Buffer, + mount_matrix: MountMatrix, + accel: Option, + gyro: Option, + /// List of events that should not be generated + filtered_events: HashSet, +} + +impl Driver { + pub fn new( + name: String, + matrix: Option, + sample_rate: Option, + ) -> Result> { + // Create an IIO local context used to query for devices + let ctx = industrial_io::context::Context::new()?; + ctx.set_timeout(Duration::from_secs(5))?; + log::debug!("IIO context version: {}", ctx.version()); + + // Find the IMU device + let Some(device) = ctx.find_device(name.as_str()) else { + return Err("Failed to find device".into()); + }; + + match Self::warm_reset_iio_buffer(&device, name.clone()) { + Ok(_) => (), + Err(_) => { + log::warn!("Failed to reset device, it may not permit grab."); + } + }; + + log::debug!("Creating IIO IMU driver instance for {:?}", device.name()); + + // Try finding the mount matrix to determine how sensors were mounted inside + // the device. + // https://github.com/torvalds/linux/blob/master/Documentation/devicetree/bindings/iio/mount-matrix.txt + let mount_matrix = if let Some(matrix) = matrix { + // Use the provided mount matrix if it is defined + matrix + } else if let Some(mount) = device.find_channel("mount", Direction::Input) { + // Read from the matrix + let matrix_str = mount.attr_read_str("matrix")?; + log::debug!("Found mount matrix: {matrix_str}"); + let matrix = MountMatrix::new(matrix_str)?; + log::debug!("Decoded mount matrix: {matrix}"); + matrix + } else { + MountMatrix::default() + }; + + // Find all accelerometer and gyro channels + let (accel_ch, accel_info) = get_channels_with_type(&device, ChannelType::Accel); + + for attr in &accel_info { + log::debug!("Found accel_info: {:?}", attr); + } + + let (gyro_ch, gyro_info) = get_channels_with_type(&device, ChannelType::AnglVel); + + for attr in &gyro_info { + log::debug!("Found gyro_info: {:?}", attr); + } + + // Log device attributes + for attr in device.attributes() { + log::trace!("Found device attribute: {:?}", attr) + } + + // Log all found channels + for channel in device.channels() { + log::trace!("Found channel: {:?} {:?}", channel.id(), channel.name()); + log::trace!(" Is output: {}", channel.is_output()); + log::trace!(" Is scan element: {}", channel.is_scan_element()); + for attr in channel.attrs() { + log::trace!(" Found attribute: {:?}", attr); + } + } + + // Bind a trigger for this device + if let Some(trigger) = ctx.devices().find(|d| { + if let (Some(current), Some(iter)) = (device.name().as_deref(), d.name().as_deref()) { + current != iter && iter.contains(current) && d.is_trigger() + } else { + false + } + }) { + match device.set_trigger(&trigger) { + Ok(_) => log::debug!("Set trigger for IMU to {:?}", trigger.name()), + Err(e) => log::warn!( + "Failed to set trigger for IMU: {:?}. Assuming trigger already set.", + e + ), + }; + } else { + log::debug!("Unable to find trigger for {name:?}"); + }; + + // Enable scan elements for channels and request a higher sampling rate + for channel in device.channels() { + let id = channel.id().unwrap_or_default(); + + if channel.has_attr("sampling_frequency") { + let current_rate = match channel.attr_read_float("sampling_frequency") { + Ok(v) => v, + Err(e) => { + log::warn!( + "Unable to read sample rate for channel {}: {:?}", + id.clone(), + e + ); + 4.0 + } + }; + + let channel_type = channel.channel_type(); + if let Err(err) = set_sample_rate_or_default( + &device, + id.clone(), + current_rate, + &channel, + channel_type, + sample_rate, + ) { + log::warn!("Failed to set sample rate: {err}, falling back to max available"); + set_sample_rate_max(&device, id.clone(), current_rate, &channel, channel_type); + } + } + + if channel.is_scan_element() { + log::debug!("Channel {:?} in not enabled, enabling.", channel.id()); + channel.enable(); + } + log::debug!( + "Channel {:?} enabled state is {:?}", + channel.id(), + channel.is_enabled() + ); + } + + // Create buffer + let buffer = match device.create_buffer(64, false) { + Ok(b) => b, + Err(e) => { + log::error!("create_buffer failed: {:?}", e); + return Err(e.into()); + } + }; + + // Ensure minimum latency + if buffer.has_attr("watermark") { + buffer.attr_write_int("watermark", 1)?; + println!("Successfully set buffer watermark to 1 sample."); + } + buffer.set_blocking_mode(false)?; + + // Build channel data + let accel = Self::build_group(&accel_ch, &accel_info, "accel"); + let gyro = Self::build_group(&gyro_ch, &gyro_info, "anglvel"); + + log::debug!("accel present: {:?}", accel.is_some()); + log::debug!("gyro present: {:?}", gyro.is_some()); + + Ok(Self { + _device: device, + buffer, + mount_matrix, + accel, + gyro, + filtered_events: Default::default(), + }) + } + + fn warm_reset_iio_buffer(device: &Device, name: String) -> std::io::Result<()> { + let buffer_path = format!("/sys/bus/iio/devices/{}/buffer/enable", name); + + log::debug!("Executing dynamic buffer cycle reset..."); + + // Force the kernel trigger assignment to clear out any stale, locked states + if let Err(e) = device.remove_trigger() { + log::debug!("Trigger already clear or handled: {:?}", e); + } + + // Explicitly write 0 to sysfs to tear down any stuck hardware state machines + if let Err(e) = fs::write(&buffer_path, "0") { + log::debug!("Sysfs buffer disable skipped or already 0: {:?}", e); + } + + thread::sleep(Duration::from_millis(100)); + + // Cycle a mock single-sample buffer execution to flush out the kernel ring boundaries + let mut buffer_init = device.create_buffer(1, false); + + if matches!(&buffer_init, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EBUSY as i32) + { + log::debug!("Unable to clear buffer with IOCTL, attempting forced drop"); + fs::write(&buffer_path, "0")?; + thread::sleep(Duration::from_millis(100)); + buffer_init = device.create_buffer(1, false); + } + + // Drop the initialization handles completely before building the true runtime configuration + let tmp_buffer = buffer_init; + std::mem::drop(tmp_buffer); + + log::debug!("Dynamic reset complete. udev node preserved."); + Ok(()) + } + + fn build_axis( + channels: &HashMap, + info: &HashMap, + key: &str, + ) -> Option { + let channel = match channels.get(key) { + Some(c) => c.clone(), + None => return None, + }; + + let info = match info.get(key) { + Some(i) => i.clone(), + None => return None, + }; + + Some(Axis { channel, info }) + } + + fn build_group( + channels: &HashMap, + info: &HashMap, + prefix: &str, + ) -> Option { + let x = Self::build_axis(channels, info, &format!("{prefix}_x"))?; + let y = Self::build_axis(channels, info, &format!("{prefix}_y"))?; + let z = Self::build_axis(channels, info, &format!("{prefix}_z"))?; + + Some(ImuGroup { x, y, z }) + } + + //TODO: Using InputPlumber Capability enum prevents this driver from having the ability to be + //a standalone crate. When this driver is eventually separated, refactor the Event type to + //follow the pattern DeviceEvent(Event, Value) and create a match table for + //Capability->Event/Event->Capability in the SourceDriver implementation. + pub fn update_filtered_events(&mut self, events: HashSet) { + self.filtered_events = events; + } + + /// Poll the device for data + pub fn poll(&mut self) -> Result, Box> { + let mut events = Vec::new(); + let refill_res = self.buffer.refill(); + + // EAGAIN is IIO telling us refill would block but we're set to non-blocking. This happens + // because no data is available in the buffer. We can trigger a single read of one channel + // to fill the buffer and get it on the next loop. + if matches!(&refill_res, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EAGAIN as i32) + || matches!(&refill_res, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EBUSY as i32) + { + log::debug!("Buffer has no data. Forcing read to fill buffer"); + if let Some(accel) = &self.accel { + match accel.x.channel.attr_read_int("raw") { + Ok(_) => return Ok(events), + Err(e) => return Err(format!("Unable to probe accel channel: {:?}", e).into()), + }; + }; + if let Some(gyro) = &self.gyro { + match gyro.x.channel.attr_read_int("raw") { + Ok(_) => return Ok(events), + Err(e) => return Err(format!("Unable to probe gyro channel: {:?}", e).into()), + }; + }; + return Ok(events); + } else if let Err(e) = refill_res { + log::error!("True buffer error: {:?}", e); + return Ok(events); + } + + if self.accel.is_some() + && !self + .filtered_events + .contains(&Capability::Accelerometer(Source::Center)) + { + events.extend(self.poll_accel()?); + } + + if self.gyro.is_some() + && !self + .filtered_events + .contains(&Capability::Gyroscope(Source::Center)) + { + events.extend(self.poll_gyro()?); + } + log::debug!("Got IIO IMU events: {:?}", events); + + Ok(events) + } + + /// Polls all the channels from the accelerometer and drains the buffer + fn poll_accel(&self) -> Result, Box> { + let accel = match &self.accel { + Some(a) => a, + None => return Ok(Vec::new()), + }; + + let mut events = Vec::new(); + let mut x_iter = self.buffer.channel_iter::(&accel.x.channel); + let mut y_iter = self.buffer.channel_iter::(&accel.y.channel); + let mut z_iter = self.buffer.channel_iter::(&accel.z.channel); + + // Continuous loop to exhaustively empty the kernel's filled buffer window + while let (Some(x), Some(y), Some(z)) = (x_iter.next(), y_iter.next(), z_iter.next()) { + log::debug!( + "Raw accel X data: {x}, offset: {:?}, scale: {:?}", + accel.x.info.offset, + accel.x.info.scale + ); + log::debug!( + "Raw accel Y data: {y}, offset: {:?}, scale: {:?}", + accel.y.info.offset, + accel.y.info.scale + ); + log::debug!( + "Raw accel Z data: {z}, offset: {:?}, scale: {:?}", + accel.z.info.offset, + accel.z.info.scale + ); + let mut out = AxisData { + roll: (*x as f64 + accel.x.info.offset as f64) * accel.x.info.scale, + pitch: (*y as f64 + accel.y.info.offset as f64) * accel.y.info.scale, + yaw: (*z as f64 + accel.z.info.offset as f64) * accel.z.info.scale, + }; + self.rotate_value(&mut out); + events.push(Event::Accelerometer(out)); + } + + Ok(events) + } + + /// Polls all the channels from the gyro and drains the buffer + fn poll_gyro(&self) -> Result, Box> { + let gyro = match &self.gyro { + Some(g) => g, + None => return Ok(Vec::new()), + }; + + let mut events = Vec::new(); + let mut x_iter = self.buffer.channel_iter::(&gyro.x.channel); + let mut y_iter = self.buffer.channel_iter::(&gyro.y.channel); + let mut z_iter = self.buffer.channel_iter::(&gyro.z.channel); + + // Continuous loop to exhaustively empty the kernel's filled buffer window + while let (Some(x), Some(y), Some(z)) = (x_iter.next(), y_iter.next(), z_iter.next()) { + log::debug!( + "Raw gyro X data: {x}, offset: {:?}, scale: {:?}", + gyro.x.info.offset, + gyro.x.info.scale + ); + log::debug!( + "Raw gyro Y data: {y}, offset: {:?}, scale: {:?}", + gyro.y.info.offset, + gyro.y.info.scale + ); + log::debug!( + "Raw gyro Z data: {z}, offset: {:?}, scale: {:?}", + gyro.z.info.offset, + gyro.z.info.scale + ); + let mut out = AxisData { + roll: (*x as f64 + gyro.x.info.offset as f64) * gyro.x.info.scale, + pitch: (*y as f64 + gyro.y.info.offset as f64) * gyro.y.info.scale, + yaw: (*z as f64 + gyro.z.info.offset as f64) * gyro.z.info.scale, + }; + self.rotate_value(&mut out); + events.push(Event::Gyro(out)); + } + + Ok(events) + } + + /// Rotate the given axis data according to the mount matrix. This is used + /// to calculate the final value according to the sensor oritentation. + // Values are intended to be multiplied as: + // x' = mxx * x + myx * y + mzx * z + // y' = mxy * x + myy * y + mzy * z + // z' = mxz * x + myz * y + mzz * z + fn rotate_value(&self, value: &mut AxisData) { + let x = value.roll; + let y = value.pitch; + let z = value.yaw; + let mxx = self.mount_matrix.x.0; + let myx = self.mount_matrix.x.1; + let mzx = self.mount_matrix.x.2; + let mxy = self.mount_matrix.y.0; + let myy = self.mount_matrix.y.1; + let mzy = self.mount_matrix.y.2; + let mxz = self.mount_matrix.z.0; + let myz = self.mount_matrix.z.1; + let mzz = self.mount_matrix.z.2; + value.roll = mxx * x + myx * y + mzx * z; + value.pitch = mxy * x + myy * y + mzy * z; + value.yaw = mxz * x + myz * y + mzz * z; + } +} + +/// Returns all channels and channel information from the given device matching +/// the given channel type. +fn get_channels_with_type( + device: &Device, + channel_type: ChannelType, +) -> (HashMap, HashMap) { + let mut channels = HashMap::new(); + let mut channel_info = HashMap::new(); + device + .channels() + .filter(|channel| channel.channel_type() == channel_type) + .for_each(|channel| { + let id = channel.id().unwrap_or_default(); + log::debug!("Found channel: {id}"); + + // Get the offset of the axis + let offset = match channel.attr_read_int("offset") { + Ok(v) => v, + Err(e) => { + log::debug!("Unable to read offset for channel {id}: {:?}", e); + 0 + } + }; + + // Get the sample rate of the axis + let sample_rate = channel + .attr_read_float("sampling_frequency") + .unwrap_or_else(|_| { + let fallback_attr = match channel_type { + ChannelType::Accel => "in_accel_sampling_frequency", + ChannelType::AnglVel => "in_anglvel_sampling_frequency", + // Add other types here if your framework scales them later + _ => "", + }; + + if !fallback_attr.is_empty() { + device.attr_read_float(fallback_attr).unwrap_or(1.0) + } else { + 4.0 + } + }); + + let sample_rates_avail = read_sample_rates_available(id.clone(), sample_rate, &channel); + // Get the scale of the axis to normalize values to meters per second^2 or rads per second + let scale = channel.attr_read_float("scale").unwrap_or_else(|_| { + let fallback_attr = match channel_type { + ChannelType::Accel => "in_accel_scale", + ChannelType::AnglVel => "in_anglvel_scale", + // Add other types here if your framework scales them later + _ => "", + }; + + if !fallback_attr.is_empty() { + device.attr_read_float(fallback_attr).unwrap_or(1.0) + } else { + 1.0 + } + }); + + let scales_avail = read_scales_available(id.clone(), scale, &channel); + + let info = AxisInfo { + offset, + sample_rate, + sample_rates_avail, + scale, + scales_avail, + }; + channel_info.insert(id.clone(), info); + channels.insert(id, channel); + }); + + (channels, channel_info) +} + +/// Try to set a specific or default sampling rate. Returns Err if the +/// requested rate is not in the hardware's available list. +fn set_sample_rate_or_default( + device: &Device, + id: String, + current_rate: f64, + channel: &Channel, + channel_type: ChannelType, + target_rate: Option, +) -> Result<(), Box> { + let rate = target_rate.unwrap_or(DEFAULT_SAMPLE_RATE); + + let avail: Vec = read_sample_rates_available(id.clone(), current_rate, channel); + + if !avail.is_empty() && !avail.contains(&rate) { + return Err(format!("Requested {rate} Hz not in available rates: {avail:?}").into()); + } + + write_sample_rate(device, channel, channel_type, rate) +} + +/// Set sampling rate to the maximum reported by the hardware. +/// Falls back to DEFAULT_SAMPLE_RATE if no available rates are reported. +fn set_sample_rate_max( + device: &Device, + id: String, + current_rate: f64, + channel: &Channel, + channel_type: ChannelType, +) { + let avail = read_sample_rates_available(id.clone(), current_rate, channel); + if avail.len() == 1 { + log::info!("Channel {id} only has 1 sample rate available: {current_rate}"); + return; + } + + let rate = if avail.is_empty() { + log::warn!("No available sample rates reported, using default {DEFAULT_SAMPLE_RATE} Hz"); + DEFAULT_SAMPLE_RATE + } else { + let max = avail.iter().cloned().fold(f64::NEG_INFINITY, f64::max); + log::info!("Using max available sample rate: {max} Hz"); + max + }; + + if rate == current_rate { + log::info!("Channel {} is currently at {}", id.clone(), rate.clone()); + return; + } + + if let Err(err) = write_sample_rate(device, channel, channel_type, rate) { + log::warn!("Failed to set max sample rate: {err}"); + } +} + +/// Write a sampling rate to the device. Tries per-channel for BMI-style, +/// and device-level for HID Sensor Hub. +fn write_sample_rate( + device: &Device, + channel: &Channel, + channel_type: ChannelType, + rate: f64, +) -> Result<(), Box> { + let Some(name) = device.name() else { + return Err("Unable to find name for IIO IMU device. Can't set sample rate".into()); + }; + + if ["gyro_3d", "accel_3d"].contains(&name.as_str()) { + write_sample_rate_to_device(device, channel_type, rate) + } else { + write_sample_rate_per_channel(channel, rate) + } +} + +fn write_sample_rate_to_device( + device: &Device, + channel_type: ChannelType, + rate: f64, +) -> Result<(), Box> { + log::info!("Setting rate to {rate} on IMU IIO Device"); + + let attr = match channel_type { + ChannelType::Accel => "in_accel_sampling_frequency", + ChannelType::AnglVel => "in_anglvel_sampling_frequency", + _ => return Err("Unknown channel type".into()), + }; + + device.attr_write_float(attr, rate)?; + match device.attr_read_float(attr) { + Ok(actual) => log::info!("Set device-level {attr} to {actual} Hz"), + Err(err) => log::warn!("Set {attr} but read-back failed: {err}, assuming {rate} Hz"), + } + Ok(()) +} +fn write_sample_rate_per_channel( + channel: &Channel, + rate: f64, +) -> Result<(), Box> { + log::info!("Setting rate to {rate} on all channels of IIO IMU Device"); + + let id = channel.id().unwrap_or_else(|| "Unknown".to_string()); + match channel.attr_write_float("sampling_frequency", rate) { + Ok(_) => { + match channel.attr_read_float("sampling_frequency") { + Ok(actual) => { + log::info!("Set sampling_frequency to {actual} Hz via channel {id}") + } + Err(err) => log::warn!( + "Set sampling_frequency for {id} but read-back failed: {err}, assuming {rate} Hz" + ), + } + return Ok(()); + } + Err(err) => { + log::warn!("Per-channel sampling_frequency write failed for {id}: {err}"); + } + } + Ok(()) +} + +/// Read the list of supported sampling rates from the hardware. +fn read_sample_rates_available(id: String, sample_rate: f64, channel: &Channel) -> Vec { + match channel.has_attr("sampling_frequency_available") { + true => { + match channel.attr_read_str("sampling_frequency_available") { + Ok(v) => { + let mut all_rates = Vec::new(); + for val in v.split_whitespace() { + // convert the string into f64 + all_rates.push(val.parse::().unwrap()); + } + all_rates + } + Err(e) => { + log::warn!( + "Unable to read available sample rates for channel {id}: {:?}", + e + ); + vec![sample_rate] + } + } + } + false => { + log::warn!( + "Unable to read available sample rates for channel {id}: attribute not available" + ); + vec![sample_rate] + } + } +} + +/// Read the list of supported scales from the hardware. +fn read_scales_available(id: String, scale: f64, channel: &Channel) -> Vec { + match channel.has_attr("scale_available") { + true => { + match channel.attr_read_str("scale_available") { + Ok(v) => { + let mut all_scales = Vec::new(); + for val in v.split_whitespace() { + // convert the string into f64 + all_scales.push(val.parse::().unwrap()); + } + all_scales + } + Err(e) => { + log::warn!( + "Unable to read available sample rates for channel {id}: {:?}", + e + ); + vec![scale] + } + } + } + false => { + log::warn!( + "Unable to read available sample rates for channel {id}: attribute not available" + ); + vec![scale] + } + } +} diff --git a/src/drivers/iio_imu/driver.rs b/src/drivers/iio_imu/driver.rs deleted file mode 100644 index 19422748..00000000 --- a/src/drivers/iio_imu/driver.rs +++ /dev/null @@ -1,496 +0,0 @@ -use std::{ - collections::{HashMap, HashSet}, - error::Error, - fs::File, - io::{self, BufRead, BufReader}, -}; - -use industrial_io::{Channel, ChannelType, Device, Direction}; - -use crate::{ - drivers::iio_imu::info::MountMatrix, - input::capability::{Capability, Source}, -}; - -use super::{ - event::{AxisData, Event}, - info::AxisInfo, -}; - -const DEFAULT_SAMPLE_RATE: f64 = 200.0; - -/// Driver for reading IIO IMU data -pub struct Driver { - _device: Device, // must outlive Channel raw pointers - mount_matrix: MountMatrix, - accel: HashMap, - accel_info: HashMap, - gyro: HashMap, - gyro_info: HashMap, - /// List of events that should not be generated - filtered_events: HashSet, -} - -impl Driver { - pub fn new( - id: String, - name: String, - matrix: Option, - sample_rate: Option, - ) -> Result> { - log::debug!("Creating IIO IMU driver instance for {name}"); - - // Create an IIO local context used to query for devices - let ctx = industrial_io::context::Context::new()?; - log::debug!("IIO context version: {}", ctx.version()); - - // Find the IMU device - let Some(device) = ctx.find_device(id.as_str()) else { - return Err("Failed to find device".into()); - }; - - // Try finding the mount matrix to determine how sensors were mounted inside - // the device. - // https://github.com/torvalds/linux/blob/master/Documentation/devicetree/bindings/iio/mount-matrix.txt - let mount_matrix = if let Some(matrix) = matrix { - // Use the provided mount matrix if it is defined - matrix - } else if let Some(mount) = device.find_channel("mount", Direction::Input) { - // Read from the matrix - let matrix_str = mount.attr_read_str("matrix")?; - log::debug!("Found mount matrix: {matrix_str}"); - let matrix = MountMatrix::new(matrix_str)?; - log::debug!("Decoded mount matrix: {matrix}"); - matrix - } else { - MountMatrix::default() - }; - - // Find all accelerometer and gyro channels and insert them into a hashmap - let (accel, accel_info) = get_channels_with_type(&device, ChannelType::Accel); - for attr in &accel_info { - log::debug!("Found accel_info: {:?}", attr); - } - let (gyro, gyro_info) = get_channels_with_type(&device, ChannelType::AnglVel); - for attr in &gyro_info { - log::debug!("Found gyro_info: {:?}", attr); - } - - // Log device attributes - for attr in device.attributes() { - log::trace!("Found device attribute: {:?}", attr) - } - - // Log all found channels - for channel in device.channels() { - log::trace!("Found channel: {:?} {:?}", channel.id(), channel.name()); - log::trace!(" Is output: {}", channel.is_output()); - log::trace!(" Is scan element: {}", channel.is_scan_element()); - for attr in channel.attrs() { - log::trace!(" Found attribute: {:?}", attr); - } - } - - // Request a higher sampling rate - for (channels, ch_type) in [(&accel, ChannelType::Accel), (&gyro, ChannelType::AnglVel)] { - if channels.is_empty() { - continue; - } - if let Err(err) = - set_sample_rate_or_default(&device, channels, ch_type, sample_rate) - { - log::warn!("Failed to set sample rate: {err}, falling back to max available"); - set_sample_rate_max(&device, channels, ch_type); - } - } - - Ok(Self { - _device: device, - mount_matrix, - accel, - accel_info, - gyro, - gyro_info, - filtered_events: Default::default(), - }) - } - - //TODO: Using InputPlumber Capability enum prevents this driver from having the ability to be - //a standalone crate. When this driver is eventually separated, refactor the Event type to - //follow the pattern DeviceEvent(Event, Value) and create a match table for - //Capability->Event/Event->Capability in the SourceDriver implementation. - pub fn update_filtered_events(&mut self, events: HashSet) { - self.filtered_events = events; - } - - pub fn get_default_event_filter( - &self, - ) -> Result, Box> { - let filtered_events = match is_driver_loaded("hid_lenovo_go") { - Ok(true) => { - log::debug!("Found hid-lenovo-go driver. Disabling internal gyroscope."); - HashSet::from([ - Capability::Accelerometer(Source::Center), - Capability::Gyroscope(Source::Center), - ]) - } - Ok(false) => { - log::debug!("Did not find hid-lenovo-go driver. Enabling internal gyroscope."); - HashSet::new() - } - Err(e) => { - return Err(format!("Failed to read '/proc/modules': {e:?}").into()); - } - }; - Ok(filtered_events) - } - - /// Poll the device for data - pub fn poll(&self) -> Result, Box> { - let mut events = vec![]; - - // Read from the accelerometer - if !self - .filtered_events - .contains(&Capability::Accelerometer(Source::Center)) - { - if let Some(event) = self.poll_accel()? { - events.push(event); - } - } - - // Read from the gyro - if !self - .filtered_events - .contains(&Capability::Gyroscope(Source::Center)) - { - if let Some(event) = self.poll_gyro()? { - events.push(event); - } - } - - Ok(events) - } - - /// Polls all the channels from the accelerometer - fn poll_accel(&self) -> Result, Box> { - if self.accel.is_empty() { - return Ok(None); - } - - // Read from each accel channel - let mut accel_input = AxisData::default(); - for (id, channel) in self.accel.iter() { - // Get the info for the axis and read the data - let Some(info) = self.accel_info.get(id) else { - continue; - }; - let data = channel.attr_read_int("raw")?; - - // processed_value = (raw + offset) * scale - let value = (data + info.offset) as f64 * info.scale; - if id.ends_with('x') { - accel_input.roll = value; - } - if id.ends_with('y') { - accel_input.pitch = value; - } - if id.ends_with('z') { - accel_input.yaw = value; - } - } - self.rotate_value(&mut accel_input); - - Ok(Some(Event::Accelerometer(accel_input))) - } - - /// Polls all the channels from the gyro - fn poll_gyro(&self) -> Result, Box> { - if self.gyro.is_empty() { - return Ok(None); - } - - let mut gyro_input = AxisData::default(); - for (id, channel) in self.gyro.iter() { - // Get the info for the axis and read the data - let Some(info) = self.gyro_info.get(id) else { - continue; - }; - let data = channel.attr_read_int("raw")?; - - // processed_value = (raw + offset) * scale - let value = (data + info.offset) as f64 * info.scale; - - if id.ends_with('x') { - gyro_input.roll = value; - } - if id.ends_with('y') { - gyro_input.pitch = value; - } - if id.ends_with('z') { - gyro_input.yaw = value; - } - } - self.rotate_value(&mut gyro_input); - - Ok(Some(Event::Gyro(gyro_input))) - } - - /// Rotate the given axis data according to the mount matrix. This is used - /// to calculate the final value according to the sensor oritentation. - // Values are intended to be multiplied as: - // x' = mxx * x + myx * y + mzx * z - // y' = mxy * x + myy * y + mzy * z - // z' = mxz * x + myz * y + mzz * z - fn rotate_value(&self, value: &mut AxisData) { - let x = value.roll; - let y = value.pitch; - let z = value.yaw; - let mxx = self.mount_matrix.x.0; - let myx = self.mount_matrix.x.1; - let mzx = self.mount_matrix.x.2; - let mxy = self.mount_matrix.y.0; - let myy = self.mount_matrix.y.1; - let mzy = self.mount_matrix.y.2; - let mxz = self.mount_matrix.z.0; - let myz = self.mount_matrix.z.1; - let mzz = self.mount_matrix.z.2; - value.roll = mxx * x + myx * y + mzx * z; - value.pitch = mxy * x + myy * y + mzy * z; - value.yaw = mxz * x + myz * y + mzz * z; - } -} - -/// Returns all channels and channel information from the given device matching -/// the given channel type. -fn get_channels_with_type( - device: &Device, - channel_type: ChannelType, -) -> (HashMap, HashMap) { - let mut channels = HashMap::new(); - let mut channel_info = HashMap::new(); - device - .channels() - .filter(|channel| channel.channel_type() == channel_type) - .for_each(|channel| { - let Some(id) = channel.id() else { - log::warn!("Unable to get channel id for channel: {:?}", channel); - return; - }; - log::debug!("Found channel: {id}"); - - // Get the offset of the axis - let offset = match channel.attr_read_int("offset") { - Ok(v) => v, - Err(e) => { - log::debug!("Unable to read offset for channel {id}: {:?}", e); - 0 - } - }; - - // Get the sample rate of the axis - let sample_rate = match channel.attr_read_float("sampling_frequency") { - Ok(v) => v, - Err(e) => { - log::warn!("Unable to read sample rate for channel {id}: {:?}", e); - 4.0 - } - }; - - let sample_rates_avail = match channel.attr_read_str("sampling_frequency_available") { - Ok(v) => { - let mut all_scales = Vec::new(); - for val in v.split_whitespace() { - // convert the string into f64 - all_scales.push(val.parse::().unwrap()); - } - all_scales - } - Err(e) => { - log::warn!( - "Unable to read available sample rates for channel {id}: {:?}", - e - ); - vec![4.0] - } - }; - - // Get the scale of the axis to normalize values to meters per second or rads per - // second - let scale = match channel.attr_read_float("scale") { - Ok(v) => v, - Err(e) => { - log::warn!("Unable to read scale for channel {id}: {:?}", e); - 1.0 - } - }; - - let scales_avail = match channel.attr_read_str("scale_available") { - Ok(v) => { - let mut all_scales = Vec::new(); - for val in v.split_whitespace() { - // convert the string into f64 - all_scales.push(val.parse::().unwrap()); - } - all_scales - } - Err(e) => { - log::warn!("Unable to read available scales for channel {id}: {:?}", e); - vec![1.0] - } - }; - - let info = AxisInfo { - offset, - sample_rate, - sample_rates_avail, - scale, - scales_avail, - }; - channel_info.insert(id.clone(), info); - channels.insert(id, channel); - }); - - (channels, channel_info) -} - -fn is_driver_loaded(driver_name: &str) -> io::Result { - let file = File::open("/proc/modules")?; - let reader = BufReader::new(file); - - for line in reader.lines() { - let line = line?; - if line.starts_with(driver_name) { - return Ok(true); - } - } - Ok(false) -} - -/// Try to set a specific or default sampling rate. Returns Err if the -/// requested rate is not in the hardware's available list. -fn set_sample_rate_or_default( - device: &Device, - channels: &HashMap, - channel_type: ChannelType, - target_rate: Option, -) -> Result<(), Box> { - let rate = target_rate.unwrap_or(DEFAULT_SAMPLE_RATE); - let avail = read_sample_rates_available(device, channels, &channel_type); - - if !avail.is_empty() && !avail.contains(&rate) { - return Err(format!( - "Requested {rate} Hz not in available rates: {avail:?}" - ) - .into()); - } - - write_sample_rate(device, channels, channel_type, rate) -} - -/// Set sampling rate to the maximum reported by the hardware. -/// Falls back to DEFAULT_SAMPLE_RATE if no available rates are reported. -fn set_sample_rate_max( - device: &Device, - channels: &HashMap, - channel_type: ChannelType, -) { - let avail = read_sample_rates_available(device, channels, &channel_type); - let rate = if avail.is_empty() { - log::warn!( - "No available sample rates reported, using default {DEFAULT_SAMPLE_RATE} Hz" - ); - DEFAULT_SAMPLE_RATE - } else { - let max = avail.iter().cloned().fold(f64::NEG_INFINITY, f64::max); - log::info!("Using max available sample rate: {max} Hz"); - max - }; - - if let Err(err) = write_sample_rate(device, channels, channel_type, rate) { - log::warn!("Failed to set max sample rate: {err}"); - } -} - -/// Write a sampling rate to the device. Tries per-channel first (BMI-style), -/// then falls back to device-level attribute (HID Sensor Hub). -fn write_sample_rate( - device: &Device, - channels: &HashMap, - channel_type: ChannelType, - rate: f64, -) -> Result<(), Box> { - for (id, channel) in channels.iter() { - match channel.attr_write_float("sampling_frequency", rate) { - Ok(_) => { - match channel.attr_read_float("sampling_frequency") { - Ok(actual) => { - log::info!("Set sampling_frequency to {actual} Hz via channel {id}") - } - Err(err) => log::warn!( - "Set sampling_frequency for {id} but read-back failed: {err}, assuming {rate} Hz" - ), - } - return Ok(()); - } - Err(err) => { - log::warn!( - "Per-channel sampling_frequency write failed for {id}: {err}" - ); - } - } - } - - let attr = match channel_type { - ChannelType::Accel => "in_accel_sampling_frequency", - ChannelType::AnglVel => "in_anglvel_sampling_frequency", - _ => return Err("Unknown channel type".into()), - }; - - device.attr_write_float(attr, rate)?; - match device.attr_read_float(attr) { - Ok(actual) => log::info!("Set device-level {attr} to {actual} Hz"), - Err(err) => log::warn!( - "Set {attr} but read-back failed: {err}, assuming {rate} Hz" - ), - } - Ok(()) -} - -/// Read the list of supported sampling rates from the hardware. -/// Tries per-channel attribute first, then device-level global attribute. -fn read_sample_rates_available( - device: &Device, - channels: &HashMap, - channel_type: &ChannelType, -) -> Vec { - for (_, channel) in channels.iter() { - if let Ok(val) = channel.attr_read_str("sampling_frequency_available") { - let rates: Vec = val - .split_whitespace() - .filter_map(|s| s.parse().ok()) - .collect(); - if !rates.is_empty() { - return rates; - } - } - } - - let attr = match channel_type { - ChannelType::Accel => "in_accel_sampling_frequency_available", - ChannelType::AnglVel => "in_anglvel_sampling_frequency_available", - _ => return vec![], - }; - - if let Ok(val) = device.attr_read_str(attr) { - let rates: Vec = val - .split_whitespace() - .filter_map(|s| s.parse().ok()) - .collect(); - if !rates.is_empty() { - return rates; - } - } - - vec![] -} diff --git a/src/drivers/iio_imu/hid_sfh_driver.rs b/src/drivers/iio_imu/hid_sfh_driver.rs new file mode 100644 index 00000000..28776fb3 --- /dev/null +++ b/src/drivers/iio_imu/hid_sfh_driver.rs @@ -0,0 +1,681 @@ +use core::{option::Option::None, time::Duration}; +use std::{ + collections::{HashMap, HashSet}, + error::Error, + fs::{self}, + thread, +}; + +use industrial_io::{Buffer, Channel, ChannelType, Device, Direction}; + +use crate::input::capability::{Capability, Source}; + +use super::{ + event::{AxisData, Event}, + info::{Axis, AxisInfo, ImuGroup, MountMatrix}, +}; + +const DEFAULT_SAMPLE_RATE: f64 = 200.0; + +/// Driver for reading IIO IMU data +pub struct Driver { + _device: Device, // must outlive Channel raw pointers + buffer: Buffer, + mount_matrix: MountMatrix, + accel: Option, + gyro: Option, + /// List of events that should not be generated + filtered_events: HashSet, +} + +impl Driver { + pub fn new( + name: String, + matrix: Option, + sample_rate: Option, + ) -> Result> { + // Create an IIO local context used to query for devices + let ctx = industrial_io::context::Context::new()?; + ctx.set_timeout(Duration::from_secs(5))?; + log::debug!("IIO context version: {}", ctx.version()); + + // Find the IMU device + let Some(device) = ctx.find_device(name.as_str()) else { + return Err("Failed to find device".into()); + }; + + match Self::warm_reset_iio_buffer(&device, name.clone()) { + Ok(_) => (), + Err(_) => { + log::warn!("Failed to reset device, it may not permit grab."); + } + }; + + log::debug!("Creating IIO IMU driver instance for {:?}", device.name()); + + // Try finding the mount matrix to determine how sensors were mounted inside + // the device. + // https://github.com/torvalds/linux/blob/master/Documentation/devicetree/bindings/iio/mount-matrix.txt + let mount_matrix = if let Some(matrix) = matrix { + // Use the provided mount matrix if it is defined + matrix + } else if let Some(mount) = device.find_channel("mount", Direction::Input) { + // Read from the matrix + let matrix_str = mount.attr_read_str("matrix")?; + log::debug!("Found mount matrix: {matrix_str}"); + let matrix = MountMatrix::new(matrix_str)?; + log::debug!("Decoded mount matrix: {matrix}"); + matrix + } else { + MountMatrix::default() + }; + + // Find all accelerometer and gyro channels + let (accel_ch, accel_info) = get_channels_with_type(&device, ChannelType::Accel); + + for attr in &accel_info { + log::debug!("Found accel_info: {:?}", attr); + } + + let (gyro_ch, gyro_info) = get_channels_with_type(&device, ChannelType::AnglVel); + + for attr in &gyro_info { + log::debug!("Found gyro_info: {:?}", attr); + } + + // Log device attributes + for attr in device.attributes() { + log::trace!("Found device attribute: {:?}", attr) + } + + // Log all found channels + for channel in device.channels() { + log::trace!("Found channel: {:?} {:?}", channel.id(), channel.name()); + log::trace!(" Is output: {}", channel.is_output()); + log::trace!(" Is scan element: {}", channel.is_scan_element()); + for attr in channel.attrs() { + log::trace!(" Found attribute: {:?}", attr); + } + } + + // Bind a trigger for this device + if let Some(trigger) = ctx.devices().find(|d| { + if let (Some(current), Some(iter)) = (device.name().as_deref(), d.name().as_deref()) { + current != iter && iter.contains(current) && d.is_trigger() + } else { + false + } + }) { + match device.set_trigger(&trigger) { + Ok(_) => log::debug!("Set trigger for IMU to {:?}", trigger.name()), + Err(e) => log::warn!( + "Failed to set trigger for IMU: {:?}. Assuming trigger already set.", + e + ), + }; + } else { + log::debug!("Unable to find trigger for {name:?}"); + }; + + // Enable scan elements for channels and request a higher sampling rate + for channel in device.channels() { + let id = channel.id().unwrap_or_default(); + + if channel.has_attr("sampling_frequency") { + let current_rate = match channel.attr_read_float("sampling_frequency") { + Ok(v) => v, + Err(e) => { + log::warn!( + "Unable to read sample rate for channel {}: {:?}", + id.clone(), + e + ); + 4.0 + } + }; + + let channel_type = channel.channel_type(); + if let Err(err) = set_sample_rate_or_default( + &device, + id.clone(), + current_rate, + &channel, + channel_type, + sample_rate, + ) { + log::warn!("Failed to set sample rate: {err}, falling back to max available"); + set_sample_rate_max(&device, id.clone(), current_rate, &channel, channel_type); + } + } + + if channel.is_scan_element() { + log::debug!("Channel {:?} in not enabled, enabling.", channel.id()); + channel.enable(); + } + log::debug!( + "Channel {:?} enabled state is {:?}", + channel.id(), + channel.is_enabled() + ); + } + + // Create buffer + let buffer = match device.create_buffer(64, false) { + Ok(b) => b, + Err(e) => { + log::error!("create_buffer failed: {:?}", e); + return Err(e.into()); + } + }; + + // Ensure minimum latency + if buffer.has_attr("watermark") { + buffer.attr_write_int("watermark", 1)?; + println!("Successfully set buffer watermark to 1 sample."); + } + buffer.set_blocking_mode(false)?; + + // Build channel data + let accel = Self::build_group(&accel_ch, &accel_info, "accel"); + let gyro = Self::build_group(&gyro_ch, &gyro_info, "anglvel"); + + log::debug!("accel present: {:?}", accel.is_some()); + log::debug!("gyro present: {:?}", gyro.is_some()); + + Ok(Self { + _device: device, + buffer, + mount_matrix, + accel, + gyro, + filtered_events: Default::default(), + }) + } + + fn warm_reset_iio_buffer(device: &Device, name: String) -> std::io::Result<()> { + let buffer_path = format!("/sys/bus/iio/devices/{}/buffer/enable", name); + + log::debug!("Executing dynamic buffer cycle reset..."); + + // Force the kernel trigger assignment to clear out any stale, locked states + if let Err(e) = device.remove_trigger() { + log::debug!("Trigger already clear or handled: {:?}", e); + } + + // Explicitly write 0 to sysfs to tear down any stuck hardware state machines + if let Err(e) = fs::write(&buffer_path, "0") { + log::debug!("Sysfs buffer disable skipped or already 0: {:?}", e); + } + + thread::sleep(Duration::from_millis(100)); + + // Cycle a mock single-sample buffer execution to flush out the kernel ring boundaries + let mut buffer_init = device.create_buffer(1, false); + + if matches!(&buffer_init, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EBUSY as i32) + { + log::debug!("Unable to clear buffer with IOCTL, attempting forced drop"); + fs::write(&buffer_path, "0")?; + thread::sleep(Duration::from_millis(100)); + buffer_init = device.create_buffer(1, false); + } + + // Drop the initialization handles completely before building the true runtime configuration + let tmp_buffer = buffer_init; + std::mem::drop(tmp_buffer); + + log::debug!("Dynamic reset complete. udev node preserved."); + Ok(()) + } + + fn build_axis( + channels: &HashMap, + info: &HashMap, + key: &str, + ) -> Option { + let channel = match channels.get(key) { + Some(c) => c.clone(), + None => return None, + }; + + let info = match info.get(key) { + Some(i) => i.clone(), + None => return None, + }; + + Some(Axis { channel, info }) + } + + fn build_group( + channels: &HashMap, + info: &HashMap, + prefix: &str, + ) -> Option { + let x = Self::build_axis(channels, info, &format!("{prefix}_x"))?; + let y = Self::build_axis(channels, info, &format!("{prefix}_y"))?; + let z = Self::build_axis(channels, info, &format!("{prefix}_z"))?; + + Some(ImuGroup { x, y, z }) + } + + //TODO: Using InputPlumber Capability enum prevents this driver from having the ability to be + //a standalone crate. When this driver is eventually separated, refactor the Event type to + //follow the pattern DeviceEvent(Event, Value) and create a match table for + //Capability->Event/Event->Capability in the SourceDriver implementation. + pub fn update_filtered_events(&mut self, events: HashSet) { + self.filtered_events = events; + } + + /// Poll the device for data + pub fn poll(&mut self) -> Result, Box> { + let mut events = Vec::new(); + let refill_res = self.buffer.refill(); + + // EAGAIN is IIO telling us refill would block but we're set to non-blocking. This happens + // because no data is available in the buffer. We can trigger a single read of one channel + // to fill the buffer and get it on the next loop. + if matches!(&refill_res, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EAGAIN as i32) + || matches!(&refill_res, Err(industrial_io::Error::Nix(errno)) if *errno as i32 == nix::errno::Errno::EBUSY as i32) + { + log::debug!("Buffer has no data. Forcing read to fill buffer"); + if let Some(accel) = &self.accel { + match accel.x.channel.attr_read_int("raw") { + Ok(_) => return Ok(events), + Err(e) => return Err(format!("Unable to probe accel channel: {:?}", e).into()), + }; + }; + if let Some(gyro) = &self.gyro { + match gyro.x.channel.attr_read_int("raw") { + Ok(_) => return Ok(events), + Err(e) => return Err(format!("Unable to probe gyro channel: {:?}", e).into()), + }; + }; + return Ok(events); + } else if let Err(e) = refill_res { + log::error!("True buffer error: {:?}", e); + return Ok(events); + } + + if self.accel.is_some() + && !self + .filtered_events + .contains(&Capability::Accelerometer(Source::Center)) + { + events.extend(self.poll_accel()?); + } + + if self.gyro.is_some() + && !self + .filtered_events + .contains(&Capability::Gyroscope(Source::Center)) + { + events.extend(self.poll_gyro()?); + } + log::debug!("Got IIO IMU events: {:?}", events); + + Ok(events) + } + + /// Polls all the channels from the accelerometer and drains the buffer + fn poll_accel(&self) -> Result, Box> { + let accel = match &self.accel { + Some(a) => a, + None => return Ok(Vec::new()), + }; + + let mut events = Vec::new(); + let mut x_iter = self.buffer.channel_iter::(&accel.x.channel); + let mut y_iter = self.buffer.channel_iter::(&accel.y.channel); + let mut z_iter = self.buffer.channel_iter::(&accel.z.channel); + + // Continuous loop to exhaustively empty the kernel's filled buffer window + while let (Some(x), Some(y), Some(z)) = (x_iter.next(), y_iter.next(), z_iter.next()) { + log::debug!( + "Raw accel X data: {x}, offset: {:?}, scale: {:?}", + accel.x.info.offset, + accel.x.info.scale + ); + log::debug!( + "Raw accel Y data: {y}, offset: {:?}, scale: {:?}", + accel.y.info.offset, + accel.y.info.scale + ); + log::debug!( + "Raw accel Z data: {z}, offset: {:?}, scale: {:?}", + accel.z.info.offset, + accel.z.info.scale + ); + let mut out = AxisData { + roll: (*x as f64 + accel.x.info.offset as f64) * accel.x.info.scale, + pitch: (*y as f64 + accel.y.info.offset as f64) * accel.y.info.scale, + yaw: (*z as f64 + accel.z.info.offset as f64) * accel.z.info.scale, + }; + self.rotate_value(&mut out); + events.push(Event::Accelerometer(out)); + } + + Ok(events) + } + + /// Polls all the channels from the gyro and drains the buffer + fn poll_gyro(&self) -> Result, Box> { + let gyro = match &self.gyro { + Some(g) => g, + None => return Ok(Vec::new()), + }; + + let mut events = Vec::new(); + let mut x_iter = self.buffer.channel_iter::(&gyro.x.channel); + let mut y_iter = self.buffer.channel_iter::(&gyro.y.channel); + let mut z_iter = self.buffer.channel_iter::(&gyro.z.channel); + + // Continuous loop to exhaustively empty the kernel's filled buffer window + while let (Some(x), Some(y), Some(z)) = (x_iter.next(), y_iter.next(), z_iter.next()) { + log::debug!( + "Raw gyro X data: {x}, offset: {:?}, scale: {:?}", + gyro.x.info.offset, + gyro.x.info.scale + ); + log::debug!( + "Raw gyro Y data: {y}, offset: {:?}, scale: {:?}", + gyro.y.info.offset, + gyro.y.info.scale + ); + log::debug!( + "Raw gyro Z data: {z}, offset: {:?}, scale: {:?}", + gyro.z.info.offset, + gyro.z.info.scale + ); + let mut out = AxisData { + roll: (*x as f64 + gyro.x.info.offset as f64) * gyro.x.info.scale, + pitch: (*y as f64 + gyro.y.info.offset as f64) * gyro.y.info.scale, + yaw: (*z as f64 + gyro.z.info.offset as f64) * gyro.z.info.scale, + }; + self.rotate_value(&mut out); + events.push(Event::Gyro(out)); + } + + Ok(events) + } + + /// Rotate the given axis data according to the mount matrix. This is used + /// to calculate the final value according to the sensor oritentation. + // Values are intended to be multiplied as: + // x' = mxx * x + myx * y + mzx * z + // y' = mxy * x + myy * y + mzy * z + // z' = mxz * x + myz * y + mzz * z + fn rotate_value(&self, value: &mut AxisData) { + let x = value.roll; + let y = value.pitch; + let z = value.yaw; + let mxx = self.mount_matrix.x.0; + let myx = self.mount_matrix.x.1; + let mzx = self.mount_matrix.x.2; + let mxy = self.mount_matrix.y.0; + let myy = self.mount_matrix.y.1; + let mzy = self.mount_matrix.y.2; + let mxz = self.mount_matrix.z.0; + let myz = self.mount_matrix.z.1; + let mzz = self.mount_matrix.z.2; + value.roll = mxx * x + myx * y + mzx * z; + value.pitch = mxy * x + myy * y + mzy * z; + value.yaw = mxz * x + myz * y + mzz * z; + } +} + +/// Returns all channels and channel information from the given device matching +/// the given channel type. +fn get_channels_with_type( + device: &Device, + channel_type: ChannelType, +) -> (HashMap, HashMap) { + let mut channels = HashMap::new(); + let mut channel_info = HashMap::new(); + device + .channels() + .filter(|channel| channel.channel_type() == channel_type) + .for_each(|channel| { + let id = channel.id().unwrap_or_default(); + log::debug!("Found channel: {id}"); + + // Get the offset of the axis + let offset = match channel.attr_read_int("offset") { + Ok(v) => v, + Err(e) => { + log::debug!("Unable to read offset for channel {id}: {:?}", e); + 0 + } + }; + + // Get the sample rate of the axis + let sample_rate = channel + .attr_read_float("sampling_frequency") + .unwrap_or_else(|_| { + let fallback_attr = match channel_type { + ChannelType::Accel => "in_accel_sampling_frequency", + ChannelType::AnglVel => "in_anglvel_sampling_frequency", + // Add other types here if your framework scales them later + _ => "", + }; + + if !fallback_attr.is_empty() { + device.attr_read_float(fallback_attr).unwrap_or(1.0) + } else { + 4.0 + } + }); + + let sample_rates_avail = read_sample_rates_available(id.clone(), sample_rate, &channel); + // Get the scale of the axis to normalize values to meters per second^2 or rads per second + let scale = channel.attr_read_float("scale").unwrap_or_else(|_| { + let fallback_attr = match channel_type { + ChannelType::Accel => "in_accel_scale", + ChannelType::AnglVel => "in_anglvel_scale", + // Add other types here if your framework scales them later + _ => "", + }; + + if !fallback_attr.is_empty() { + device.attr_read_float(fallback_attr).unwrap_or(1.0) + } else { + 1.0 + } + }); + + let scales_avail = read_scales_available(id.clone(), scale, &channel); + + let info = AxisInfo { + offset, + sample_rate, + sample_rates_avail, + scale, + scales_avail, + }; + channel_info.insert(id.clone(), info); + channels.insert(id, channel); + }); + + (channels, channel_info) +} + +/// Try to set a specific or default sampling rate. Returns Err if the +/// requested rate is not in the hardware's available list. +fn set_sample_rate_or_default( + device: &Device, + id: String, + current_rate: f64, + channel: &Channel, + channel_type: ChannelType, + target_rate: Option, +) -> Result<(), Box> { + let rate = target_rate.unwrap_or(DEFAULT_SAMPLE_RATE); + + let avail: Vec = read_sample_rates_available(id.clone(), current_rate, channel); + + if !avail.is_empty() && !avail.contains(&rate) { + return Err(format!("Requested {rate} Hz not in available rates: {avail:?}").into()); + } + + write_sample_rate(device, channel, channel_type, rate) +} + +/// Set sampling rate to the maximum reported by the hardware. +/// Falls back to DEFAULT_SAMPLE_RATE if no available rates are reported. +fn set_sample_rate_max( + device: &Device, + id: String, + current_rate: f64, + channel: &Channel, + channel_type: ChannelType, +) { + let avail = read_sample_rates_available(id.clone(), current_rate, channel); + if avail.len() == 1 { + log::info!("Channel {id} only has 1 sample rate available: {current_rate}"); + return; + } + + let rate = if avail.is_empty() { + log::warn!("No available sample rates reported, using default {DEFAULT_SAMPLE_RATE} Hz"); + DEFAULT_SAMPLE_RATE + } else { + let max = avail.iter().cloned().fold(f64::NEG_INFINITY, f64::max); + log::info!("Using max available sample rate: {max} Hz"); + max + }; + + if rate == current_rate { + log::info!("Channel {} is currently at {}", id.clone(), rate.clone()); + return; + } + + if let Err(err) = write_sample_rate(device, channel, channel_type, rate) { + log::warn!("Failed to set max sample rate: {err}"); + } +} + +/// Write a sampling rate to the device. Tries per-channel for BMI-style, +/// and device-level for HID Sensor Hub. +fn write_sample_rate( + device: &Device, + channel: &Channel, + channel_type: ChannelType, + rate: f64, +) -> Result<(), Box> { + let Some(name) = device.name() else { + return Err("Unable to find name for IIO IMU device. Can't set sample rate".into()); + }; + + if ["gyro_3d", "accel_3d"].contains(&name.as_str()) { + write_sample_rate_to_device(device, channel_type, rate) + } else { + write_sample_rate_per_channel(channel, rate) + } +} + +fn write_sample_rate_to_device( + device: &Device, + channel_type: ChannelType, + rate: f64, +) -> Result<(), Box> { + log::info!("Setting rate to {rate} on IMU IIO Device"); + + let attr = match channel_type { + ChannelType::Accel => "in_accel_sampling_frequency", + ChannelType::AnglVel => "in_anglvel_sampling_frequency", + _ => return Err("Unknown channel type".into()), + }; + + device.attr_write_float(attr, rate)?; + match device.attr_read_float(attr) { + Ok(actual) => log::info!("Set device-level {attr} to {actual} Hz"), + Err(err) => log::warn!("Set {attr} but read-back failed: {err}, assuming {rate} Hz"), + } + Ok(()) +} +fn write_sample_rate_per_channel( + channel: &Channel, + rate: f64, +) -> Result<(), Box> { + log::info!("Setting rate to {rate} on all channels of IIO IMU Device"); + + let id = channel.id().unwrap_or_else(|| "Unknown".to_string()); + match channel.attr_write_float("sampling_frequency", rate) { + Ok(_) => { + match channel.attr_read_float("sampling_frequency") { + Ok(actual) => { + log::info!("Set sampling_frequency to {actual} Hz via channel {id}") + } + Err(err) => log::warn!( + "Set sampling_frequency for {id} but read-back failed: {err}, assuming {rate} Hz" + ), + } + return Ok(()); + } + Err(err) => { + log::warn!("Per-channel sampling_frequency write failed for {id}: {err}"); + } + } + Ok(()) +} + +/// Read the list of supported sampling rates from the hardware. +fn read_sample_rates_available(id: String, sample_rate: f64, channel: &Channel) -> Vec { + match channel.has_attr("sampling_frequency_available") { + true => { + match channel.attr_read_str("sampling_frequency_available") { + Ok(v) => { + let mut all_rates = Vec::new(); + for val in v.split_whitespace() { + // convert the string into f64 + all_rates.push(val.parse::().unwrap()); + } + all_rates + } + Err(e) => { + log::warn!( + "Unable to read available sample rates for channel {id}: {:?}", + e + ); + vec![sample_rate] + } + } + } + false => { + log::warn!( + "Unable to read available sample rates for channel {id}: attribute not available" + ); + vec![sample_rate] + } + } +} + +/// Read the list of supported scales from the hardware. +fn read_scales_available(id: String, scale: f64, channel: &Channel) -> Vec { + match channel.has_attr("scale_available") { + true => { + match channel.attr_read_str("scale_available") { + Ok(v) => { + let mut all_scales = Vec::new(); + for val in v.split_whitespace() { + // convert the string into f64 + all_scales.push(val.parse::().unwrap()); + } + all_scales + } + Err(e) => { + log::warn!( + "Unable to read available sample rates for channel {id}: {:?}", + e + ); + vec![scale] + } + } + } + false => { + log::warn!( + "Unable to read available sample rates for channel {id}: attribute not available" + ); + vec![scale] + } + } +} diff --git a/src/drivers/iio_imu/info.rs b/src/drivers/iio_imu/info.rs index 0d308f0a..d742df1d 100644 --- a/src/drivers/iio_imu/info.rs +++ b/src/drivers/iio_imu/info.rs @@ -1,3 +1,4 @@ +use industrial_io::Channel; use std::{error::Error, fmt}; /// The [MountMatrix] is used to define how sensors are oriented inside a device @@ -81,3 +82,14 @@ pub struct AxisInfo { pub scale: f64, pub scales_avail: Vec, } + +pub struct Axis { + pub channel: Channel, + pub info: AxisInfo, +} + +pub struct ImuGroup { + pub x: Axis, + pub y: Axis, + pub z: Axis, +} diff --git a/src/drivers/iio_imu/mod.rs b/src/drivers/iio_imu/mod.rs index 8ea65200..3480fe41 100644 --- a/src/drivers/iio_imu/mod.rs +++ b/src/drivers/iio_imu/mod.rs @@ -1,3 +1,4 @@ -pub mod driver; +pub mod bmi_driver; pub mod event; +pub mod hid_sfh_driver; pub mod info; diff --git a/src/drivers/legos/event.rs b/src/drivers/legos/event.rs index 64e535db..01d1bfe0 100644 --- a/src/drivers/legos/event.rs +++ b/src/drivers/legos/event.rs @@ -87,9 +87,9 @@ pub enum AxisEvent { /// [InertialInput] represents the state of the IMU (x, y, z) values #[derive(Clone, Debug)] pub struct InertialInput { - pub x: i16, - pub y: i16, - pub z: i16, + pub x: f64, + pub y: f64, + pub z: f64, } /// [InertialEvent] has data from the IMU diff --git a/src/drivers/legos/imu_driver.rs b/src/drivers/legos/imu_driver.rs index 57413a84..4951d666 100644 --- a/src/drivers/legos/imu_driver.rs +++ b/src/drivers/legos/imu_driver.rs @@ -6,7 +6,7 @@ use packed_struct::{types::SizedInteger, PackedStruct}; use super::{ event::{Event, InertialEvent, InertialInput}, hid_report::{InertialDataReport, InputReportType}, - GYRO_SCALE, HID_TIMEOUT, IMU_IID, INERTIAL_PACKET_SIZE, PIDS, VID, + ACCEL_TO_SI, GPS_TO_RADS, HID_TIMEOUT, IMU_IID, INERTIAL_PACKET_SIZE, PIDS, VID, }; pub struct IMUDriver { @@ -132,9 +132,9 @@ impl IMUDriver { if state.x != old_state.x || state.y != old_state.y || state.z != old_state.z { events.push(Event::Inertia(InertialEvent::Accelerometer( InertialInput { - x: -state.x.to_primitive(), - y: -state.y.to_primitive(), - z: -state.z.to_primitive(), + x: -state.x.to_primitive() as f64 * ACCEL_TO_SI, + y: -state.y.to_primitive() as f64 * ACCEL_TO_SI, + z: -state.z.to_primitive() as f64 * ACCEL_TO_SI, }, ))) }; @@ -156,9 +156,9 @@ impl IMUDriver { if state.x != old_state.x || state.y != old_state.y || state.z != old_state.z { events.push(Event::Inertia(InertialEvent::Gyro(InertialInput { - x: -state.x.to_primitive() * GYRO_SCALE, - y: -state.y.to_primitive() * GYRO_SCALE, - z: -state.z.to_primitive() * GYRO_SCALE, + x: -state.x.to_primitive() as f64 * GPS_TO_RADS, + y: -state.y.to_primitive() as f64 * GPS_TO_RADS, + z: -state.z.to_primitive() as f64 * GPS_TO_RADS, }))) }; diff --git a/src/drivers/legos/mod.rs b/src/drivers/legos/mod.rs index aecc6eef..146d49ec 100644 --- a/src/drivers/legos/mod.rs +++ b/src/drivers/legos/mod.rs @@ -1,3 +1,5 @@ +use std::f64::consts::PI; + pub mod event; pub mod hid_report; pub mod imu_driver; @@ -20,7 +22,8 @@ const TOUCH_PACKET_SIZE: usize = 10; const XINPUT_PACKET_SIZE: usize = 32; // Input report axis ranges -pub const GYRO_SCALE: i16 = 2; +pub const ACCEL_TO_SI: f64 = 0.0980665; +pub const GPS_TO_RADS: f64 = 0.1 * (PI / 180.0); pub const PAD_FORCE_MAX: f64 = 127.0; pub const PAD_FORCE_NORMAL: u8 = 32; /* Simulated average pressure */ pub const PAD_MOTION_MAX: f64 = 400.0; diff --git a/src/drivers/steam_deck/driver.rs b/src/drivers/steam_deck/driver.rs index 87b31db9..2ace0ed7 100644 --- a/src/drivers/steam_deck/driver.rs +++ b/src/drivers/steam_deck/driver.rs @@ -1,6 +1,8 @@ use std::{error::Error, ffi::CString}; -use crate::drivers::steam_deck::{hid_report::PackedInputDataReport, ProductId, VID}; +use crate::drivers::steam_deck::{ + hid_report::PackedInputDataReport, ProductId, DECK_ACCEL_TO_SI, DECK_GYRO_TO_RADS, VID, +}; use hidapi::HidDevice; use packed_struct::{ types::{Integer, SizedInteger}, @@ -9,16 +11,12 @@ use packed_struct::{ use super::{ event::{ - AccelerometerEvent, AccelerometerInput, AxisEvent, AxisInput, BinaryInput, ButtonEvent, - Event, TouchAxisInput, TriggerEvent, TriggerInput, + AxisEvent, AxisInput, BinaryInput, ButtonEvent, Event, IntertialEvent, IntertialInput, + TouchAxisInput, TriggerEvent, TriggerInput, }, hid_report::{PackedMappingsReport, PackedRumbleReport, Register, ReportType, TrackpadMode}, }; -/// Scale to multiply accelerometer values to get in units of meters per second -pub const ACCEL_SCALE: f64 = 0.0006125; -/// Scale to multiply gyro values to get in units of degrees per second -//pub const GYRO_SCALE: f64 = 0.0625; /// Size of the HID packet const PACKET_SIZE: usize = 64; /// Timeout in milliseconds for reading an HID packet @@ -407,18 +405,20 @@ impl Driver { } // Accelerometer events - events.push(Event::Accelerometer(AccelerometerEvent::Accelerometer( - AccelerometerInput { - x: state.accel_x.to_primitive(), - y: state.accel_y.to_primitive(), - z: state.accel_z.to_primitive(), + events.push(Event::Accelerometer(IntertialEvent::Accelerometer( + IntertialInput { + // Convert to Intermediate SI units for NativeEvent handling + x: state.accel_x.to_primitive() as f64 * DECK_ACCEL_TO_SI, + y: state.accel_y.to_primitive() as f64 * DECK_ACCEL_TO_SI, + z: state.accel_z.to_primitive() as f64 * DECK_ACCEL_TO_SI, }, ))); - events.push(Event::Accelerometer(AccelerometerEvent::Attitude( - AccelerometerInput { - x: state.pitch.to_primitive(), - y: state.yaw.to_primitive(), - z: state.roll.to_primitive(), + events.push(Event::Accelerometer(IntertialEvent::Gyroscope( + IntertialInput { + // Convert to Intermediate SI units for NativeEvent handling + x: state.pitch.to_primitive() as f64 * DECK_GYRO_TO_RADS, + y: state.yaw.to_primitive() as f64 * DECK_GYRO_TO_RADS, + z: state.roll.to_primitive() as f64 * DECK_GYRO_TO_RADS, }, ))); }; diff --git a/src/drivers/steam_deck/event.rs b/src/drivers/steam_deck/event.rs index ee474802..800b1bea 100644 --- a/src/drivers/steam_deck/event.rs +++ b/src/drivers/steam_deck/event.rs @@ -2,7 +2,7 @@ #[derive(Clone, Debug)] pub enum Event { Button(ButtonEvent), - Accelerometer(AccelerometerEvent), + Accelerometer(IntertialEvent), Axis(AxisEvent), Trigger(TriggerEvent), } @@ -118,16 +118,16 @@ pub enum TriggerEvent { /// AccelerometerInput represents the state of the accelerometer (x, y, z) values #[derive(Clone, Debug)] -pub struct AccelerometerInput { - pub x: i16, - pub y: i16, - pub z: i16, +pub struct IntertialInput { + pub x: f64, + pub y: f64, + pub z: f64, } /// AccelerometerEvent has data from the accelerometer #[derive(Clone, Debug)] -pub enum AccelerometerEvent { - Accelerometer(AccelerometerInput), +pub enum IntertialEvent { + Accelerometer(IntertialInput), /// Pitch, yaw, roll - Attitude(AccelerometerInput), + Gyroscope(IntertialInput), } diff --git a/src/drivers/steam_deck/mod.rs b/src/drivers/steam_deck/mod.rs index ef5606c0..83a01578 100644 --- a/src/drivers/steam_deck/mod.rs +++ b/src/drivers/steam_deck/mod.rs @@ -19,6 +19,12 @@ pub enum ProductId { /// Vendor ID pub const VID: u16 = 0x28de; +/// Scale to multiply accelerometer values to get in units of meters per second +pub const DECK_ACCEL_TO_SI: f64 = 0.0006125; +pub const DECK_SI_TO_ACCEL: f64 = 1632.6530612244898; +pub const DECK_GYRO_TO_RADS: f64 = 0.00109083078; +pub const DECK_RADS_TO_GYRO: f64 = 916.7324722093181; + impl ProductId { pub fn to_u16(&self) -> u16 { match self { diff --git a/src/input/capability.rs b/src/input/capability.rs index 9e548560..74f01345 100644 --- a/src/input/capability.rs +++ b/src/input/capability.rs @@ -49,8 +49,6 @@ impl Capability { Gamepad::Button(button) => format!("Gamepad:Button:{button}"), Gamepad::Axis(axis) => format!("Gamepad:Axis:{axis}"), Gamepad::Trigger(trigger) => format!("Gamepad:Trigger:{trigger}"), - Gamepad::Accelerometer => "Gamepad:Accelerometer".to_string(), - Gamepad::Gyro => "Gamepad:Gyro".to_string(), Gamepad::Dial(dial) => format!("Gamepad:Dial:{dial}"), }, Capability::Mouse(mouse) => match mouse { @@ -203,31 +201,6 @@ impl From for Capability { return Capability::Gamepad(Gamepad::Trigger(trigger)); } - // Gyro - if let Some(gyro_capability) = gamepad.gyro.as_ref() { - let gyro = Gamepad::from_str(&gyro_capability.name); - if gyro.is_err() { - log::error!("Invalid or unimplemented gyro: {}", gyro_capability.name); - return Capability::NotImplemented; - } - - return Capability::Gamepad(Gamepad::Gyro); - } - - // Accelerometer - if let Some(accelerometer_capability) = gamepad.accelerometer.as_ref() { - let accelerometer = Gamepad::from_str(&accelerometer_capability.name); - if accelerometer.is_err() { - log::error!( - "Invalid or unimplemented gyro: {}", - accelerometer_capability.name - ); - return Capability::NotImplemented; - } - - return Capability::Gamepad(Gamepad::Accelerometer); - } - // Dials/wheels if let Some(dial_config) = gamepad.dial.as_ref() { let dial = GamepadDial::from_str(&dial_config.name); @@ -385,14 +358,6 @@ pub enum Gamepad { /// Gamepad Trigger typically uses a single unsigned integar value that represents /// how far a trigger has been pulled Trigger(GamepadTrigger), - /// Accelerometer events measure the current acceleration of a device. This is - /// normally used to determine which way is "down" as there will be a constant - /// acceleration towards the center of the earth at 9.8 meters per second. - /// Typical will use (x, y, z) values normalized to meters per second. - Accelerometer, - /// Gyro events measure the angular velocity of a device measured - /// with (x, y, z) values normalized to degrees per second. - Gyro, /// Dials and wheels Dial(GamepadDial), } @@ -403,8 +368,6 @@ impl fmt::Display for Gamepad { Gamepad::Button(_) => write!(f, "Button"), Gamepad::Axis(_) => write!(f, "Axis"), Gamepad::Trigger(_) => write!(f, "Trigger"), - Gamepad::Accelerometer => write!(f, "Accelerometer"), - Gamepad::Gyro => write!(f, "Gyro"), Gamepad::Dial(_) => write!(f, "Dial"), } } @@ -427,8 +390,6 @@ impl FromStr for Gamepad { "Trigger" => Ok(Gamepad::Trigger(GamepadTrigger::from_str( parts.join(":").as_str(), )?)), - "Accelerometer" => Ok(Gamepad::Accelerometer), - "Gyro" => Ok(Gamepad::Gyro), "Dial" => Ok(Gamepad::Dial(GamepadDial::from_str( parts.join(":").as_str(), )?)), diff --git a/src/input/composite_device/mod.rs b/src/input/composite_device/mod.rs index c8316c4a..7891a9b7 100644 --- a/src/input/composite_device/mod.rs +++ b/src/input/composite_device/mod.rs @@ -970,10 +970,7 @@ impl CompositeDevice { } } Gamepad::Dial(_) => {} - Gamepad::Axis(_) - | Gamepad::Trigger(_) - | Gamepad::Accelerometer - | Gamepad::Gyro => {} + Gamepad::Axis(_) | Gamepad::Trigger(_) => {} }, Capability::Mouse(ref t) => match t { Mouse::Motion => {} @@ -1102,28 +1099,6 @@ impl CompositeDevice { return Ok(()); } - //TODO: Temporary force of all Gyro and Accel events to legacy Gamepad:: format. - // Remove this after targets can handle translation profiles. - if matches!(cap, Capability::Accelerometer(_)) { - let event = NativeEvent::new_translated( - cap, - Capability::Gamepad(Gamepad::Accelerometer), - event.get_value(), - ); - self.targets.write_event(event).await; - return Ok(()); - } - - if matches!(cap, Capability::Gyroscope(_)) { - let event = NativeEvent::new_translated( - cap, - Capability::Gamepad(Gamepad::Gyro), - event.get_value(), - ); - self.targets.write_event(event).await; - return Ok(()); - } - // Write the event to all target devices capable of handling the event self.targets.write_event(event).await; diff --git a/src/input/event/evdev.rs b/src/input/event/evdev.rs index 318ed7dc..1882d770 100644 --- a/src/input/event/evdev.rs +++ b/src/input/event/evdev.rs @@ -117,8 +117,12 @@ impl EvdevEvent { if let Some(info) = self.abs_info { let code = self.event.code(); match AbsoluteAxisCode(code) { - AbsoluteAxisCode::ABS_Z => normalize_unsigned_value(raw_value, info.minimum(), info.maximum()), - AbsoluteAxisCode::ABS_RZ => normalize_unsigned_value(raw_value, info.minimum(), info.maximum()), + AbsoluteAxisCode::ABS_Z => { + normalize_unsigned_value(raw_value, info.minimum(), info.maximum()) + } + AbsoluteAxisCode::ABS_RZ => { + normalize_unsigned_value(raw_value, info.minimum(), info.maximum()) + } _ => normalize_signed_value(raw_value, info.minimum(), info.maximum()), } } else { @@ -584,8 +588,6 @@ fn event_type_from_capability(capability: Capability) -> Option { Gamepad::Axis(_) => Some(EventType::ABSOLUTE), Gamepad::Trigger(_) => Some(EventType::ABSOLUTE), Gamepad::Dial(_) => Some(EventType::RELATIVE), - Gamepad::Accelerometer => None, - Gamepad::Gyro => None, }, _ => None, } @@ -687,8 +689,6 @@ fn event_codes_from_capability(capability: Capability) -> Vec { GamepadTrigger::RightTouchpadForce => vec![], GamepadTrigger::RightStickForce => vec![], }, - Gamepad::Accelerometer => vec![], - Gamepad::Gyro => vec![], Gamepad::Dial(dial) => match dial { GamepadDial::LeftStickDial => { vec![RelativeAxisCode::REL_HWHEEL.0] diff --git a/src/input/event/value.rs b/src/input/event/value.rs index be091606..984a030d 100644 --- a/src/input/event/value.rs +++ b/src/input/event/value.rs @@ -194,10 +194,6 @@ impl InputValue { Gamepad::Axis(_) => self.translate_button_to_axis(target_config), // Gamepad Button -> Trigger Gamepad::Trigger(_) => Ok(self.translate_button_to_trigger()), - // Gamepad Button -> Accelerometer - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - // Gamepad Button -> Gyro - Gamepad::Gyro => Err(TranslationError::NotImplemented), Gamepad::Dial(_) => Ok(self.clone()), }, // Gamepad Button -> Mouse @@ -281,10 +277,6 @@ impl InputValue { } // Axis -> Trigger Gamepad::Trigger(_) => Err(TranslationError::NotImplemented), - // Axis -> Accelerometer - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - // Axis -> Gyro - Gamepad::Gyro => Err(TranslationError::NotImplemented), Gamepad::Dial(_) => Err(TranslationError::NotImplemented), }, // Axis -> Mouse @@ -330,10 +322,6 @@ impl InputValue { } // Trigger -> Trigger Gamepad::Trigger(_) => Ok(self.clone()), - // Trigger -> Accelerometer - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - // Trigger -> Gyro - Gamepad::Gyro => Err(TranslationError::NotImplemented), Gamepad::Dial(_) => self.translate_trigger_to_button(source_config), }, // Trigger -> Mouse @@ -359,10 +347,6 @@ impl InputValue { // Trigger -> Accelerometer Capability::Accelerometer(_) => Err(TranslationError::NotImplemented), }, - // Accelerometer -> ... - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - // Gyro -> ... - Gamepad::Gyro => Err(TranslationError::NotImplemented), // Dial mapping to -> .. Gamepad::Dial(_) => match target_cap { Capability::None => Ok(InputValue::None), @@ -373,8 +357,6 @@ impl InputValue { Gamepad::Button(_) => self.translate_dial_to_button(source_config), Gamepad::Axis(_) => Err(TranslationError::NotImplemented), Gamepad::Trigger(_) => Err(TranslationError::NotImplemented), - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - Gamepad::Gyro => Err(TranslationError::NotImplemented), Gamepad::Dial(_) => Ok(self.clone()), }, Capability::Mouse(mouse) => match mouse { @@ -416,8 +398,6 @@ impl InputValue { Gamepad::Button(_) => Ok(self.clone()), Gamepad::Axis(_) => Err(TranslationError::NotImplemented), Gamepad::Trigger(_) => Err(TranslationError::NotImplemented), - Gamepad::Accelerometer => Err(TranslationError::NotImplemented), - Gamepad::Gyro => Err(TranslationError::NotImplemented), Gamepad::Dial(_) => Ok(self.clone()), }, // Keyboard Key -> Mouse diff --git a/src/input/source/hidraw.rs b/src/input/source/hidraw.rs index 7c438b87..5ecb7d8f 100644 --- a/src/input/source/hidraw.rs +++ b/src/input/source/hidraw.rs @@ -548,9 +548,7 @@ impl HidRawDevice { } // Horipad Steam Controller - if vid == drivers::horipad_steam::driver::VID - && drivers::horipad_steam::driver::PIDS.contains(&pid) - { + if vid == drivers::horipad_steam::VID && drivers::horipad_steam::PIDS.contains(&pid) { log::info!("Detected Horipad Steam Controller"); return DriverType::HoripadSteam; } diff --git a/src/input/source/hidraw/dualsense.rs b/src/input/source/hidraw/dualsense.rs index 9abb5991..9427e10e 100644 --- a/src/input/source/hidraw/dualsense.rs +++ b/src/input/source/hidraw/dualsense.rs @@ -4,8 +4,12 @@ use std::{collections::HashMap, error::Error}; use evdev::{FFEffectData, FFEffectKind}; use packed_struct::types::SizedInteger; -use crate::drivers::dualsense::driver::{DS5_EDGE_PID, DS5_PID, DS5_VID}; +use crate::drivers::dualsense::{ + DS5_EDGE_PID, DS5_PID, DS5_TOUCHPAD_HEIGHT, DS5_TOUCHPAD_WIDTH, DS5_VID, STICK_X_MAX, + STICK_X_MIN, STICK_Y_MAX, STICK_Y_MIN, TRIGGER_MAX, +}; use crate::drivers::steam_deck::hid_report::PackedRumbleReport; +use crate::input::capability::Source; use crate::input::output_capability::{OutputCapability, LED}; use crate::{ drivers::dualsense::{self, driver::Driver}, @@ -337,20 +341,20 @@ fn translate_event(event: dualsense::event::Event) -> NativeEvent { ), }, dualsense::event::Event::Accelerometer(accel) => match accel { - dualsense::event::AccelerometerEvent::Accelerometer(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Accelerometer), + dualsense::event::IntertialEvent::Accelerometer(value) => NativeEvent::new( + Capability::Accelerometer(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), - dualsense::event::AccelerometerEvent::Gyro(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Gyro), + dualsense::event::IntertialEvent::Gyroscope(value) => NativeEvent::new( + Capability::Gyroscope(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), }, @@ -386,12 +390,12 @@ fn translate_event(event: dualsense::event::Event) -> NativeEvent { fn normalize_axis_value(event: &dualsense::event::AxisEvent) -> InputValue { match event { dualsense::event::AxisEvent::Pad(value) => { - let max = dualsense::driver::DS5_TOUCHPAD_WIDTH; + let max = DS5_TOUCHPAD_WIDTH; let x = normalize_unsigned_value(value.x as f64, max); let x = Some(x); - let max = dualsense::driver::DS5_TOUCHPAD_HEIGHT; + let max = DS5_TOUCHPAD_HEIGHT; let y = normalize_unsigned_value(value.y as f64, max); let y = Some(y); @@ -404,26 +408,26 @@ fn normalize_axis_value(event: &dualsense::event::AxisEvent) -> InputValue { } } dualsense::event::AxisEvent::LStick(value) => { - let min = dualsense::driver::STICK_X_MIN; - let max = dualsense::driver::STICK_X_MAX; + let min = STICK_X_MIN; + let max = STICK_X_MAX; let x = normalize_signed_value(value.x as f64, min, max); let x = Some(x); - let min = dualsense::driver::STICK_Y_MIN; - let max = dualsense::driver::STICK_Y_MAX; + let min = STICK_Y_MIN; + let max = STICK_Y_MAX; let y = normalize_signed_value(value.y as f64, min, max); let y = Some(y); InputValue::Vector2 { x, y } } dualsense::event::AxisEvent::RStick(value) => { - let min = dualsense::driver::STICK_X_MIN; - let max = dualsense::driver::STICK_X_MAX; + let min = STICK_X_MIN; + let max = STICK_X_MAX; let x = normalize_signed_value(value.x as f64, min, max); let x = Some(x); - let min = dualsense::driver::STICK_Y_MIN; - let max = dualsense::driver::STICK_Y_MAX; + let min = STICK_Y_MIN; + let max = STICK_Y_MAX; let y = normalize_signed_value(value.y as f64, min, max); let y = Some(y); @@ -437,11 +441,11 @@ fn normalize_axis_value(event: &dualsense::event::AxisEvent) -> InputValue { fn normalize_trigger_value(event: &dualsense::event::TriggerEvent) -> InputValue { match event { dualsense::event::TriggerEvent::L2(value) => { - let max = dualsense::driver::TRIGGER_MAX; + let max = TRIGGER_MAX; InputValue::Float(normalize_unsigned_value(value.value as f64, max)) } dualsense::event::TriggerEvent::R2(value) => { - let max = dualsense::driver::TRIGGER_MAX; + let max = TRIGGER_MAX; InputValue::Float(normalize_unsigned_value(value.value as f64, max)) } } @@ -449,7 +453,7 @@ fn normalize_trigger_value(event: &dualsense::event::TriggerEvent) -> InputValue /// List of all capabilities that the DualSense driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -476,9 +480,9 @@ pub const CAPABILITIES: &[Capability] = &[ Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), Capability::Touchpad(Touchpad::CenterPad(Touch::Button(TouchButton::Press))), Capability::Touchpad(Touchpad::CenterPad(Touch::Button(TouchButton::Touch))), Capability::Touchpad(Touchpad::CenterPad(Touch::Motion)), diff --git a/src/input/source/hidraw/flydigi_vader_4_pro.rs b/src/input/source/hidraw/flydigi_vader_4_pro.rs index 1a8d4845..e96ee1e0 100644 --- a/src/input/source/hidraw/flydigi_vader_4_pro.rs +++ b/src/input/source/hidraw/flydigi_vader_4_pro.rs @@ -6,11 +6,10 @@ use crate::{ event, }, input::{ - capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger}, + capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source}, event::{ native::NativeEvent, - value::InputValue, - value::{normalize_signed_value, normalize_unsigned_value}, + value::{normalize_signed_value, normalize_unsigned_value, InputValue}, }, source::{InputError, SourceInputDevice, SourceOutputDevice}, }, @@ -236,19 +235,19 @@ fn translate_event(event: event::Event) -> NativeEvent { }, event::Event::Inertia(accel_event) => match accel_event { event::InertialEvent::Accelerometer(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), - event::InertialEvent::Gyro(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Gyro), + event::InertialEvent::Gyroscope(value) => NativeEvent::new( + Capability::Gyroscope(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), }, @@ -257,7 +256,7 @@ fn translate_event(event: event::Event) -> NativeEvent { /// List of all capabilities that the driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -284,7 +283,7 @@ pub const CAPABILITIES: &[Capability] = &[ Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), + Capability::Gyroscope(Source::Center), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), ]; diff --git a/src/input/source/hidraw/horipad_steam.rs b/src/input/source/hidraw/horipad_steam.rs index ee0587d2..04f955f9 100644 --- a/src/input/source/hidraw/horipad_steam.rs +++ b/src/input/source/hidraw/horipad_steam.rs @@ -1,16 +1,12 @@ use std::{error::Error, fmt::Debug}; use crate::{ - drivers::horipad_steam::{ - driver::{Driver, JOY_AXIS_MAX, JOY_AXIS_MIN, TRIGGER_AXIS_MAX}, - event, - }, + drivers::horipad_steam::{driver::Driver, event, JOY_AXIS_MAX, JOY_AXIS_MIN, TRIGGER_AXIS_MAX}, input::{ - capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger}, + capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source}, event::{ native::NativeEvent, - value::InputValue, - value::{normalize_signed_value, normalize_unsigned_value}, + value::{normalize_signed_value, normalize_unsigned_value, InputValue}, }, source::{InputError, SourceInputDevice, SourceOutputDevice}, }, @@ -228,21 +224,21 @@ fn translate_event(event: event::Event) -> NativeEvent { normalize_trigger_value(trigg), ), }, - event::Event::Inertia(accel_event) => match accel_event { + event::Event::Inertia(intertia) => match intertia { event::InertialEvent::Accelerometer(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), - event::InertialEvent::Gyro(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Gyro), + event::InertialEvent::Gyroscope(value) => NativeEvent::new( + Capability::Gyroscope(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), }, @@ -251,7 +247,7 @@ fn translate_event(event: event::Event) -> NativeEvent { /// List of all capabilities that the driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -278,7 +274,7 @@ pub const CAPABILITIES: &[Capability] = &[ Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), ]; diff --git a/src/input/source/hidraw/legos_imu.rs b/src/input/source/hidraw/legos_imu.rs index 22925859..bc65650c 100644 --- a/src/input/source/hidraw/legos_imu.rs +++ b/src/input/source/hidraw/legos_imu.rs @@ -3,7 +3,7 @@ use std::{error::Error, fmt::Debug}; use crate::{ drivers::legos::{event, imu_driver::IMUDriver}, input::{ - capability::{Capability, Gamepad}, + capability::{Capability, Source}, event::{native::NativeEvent, value::InputValue}, output_event::OutputEvent, source::{InputError, OutputError, SourceInputDevice, SourceOutputDevice}, @@ -64,19 +64,19 @@ fn translate_event(event: event::Event) -> NativeEvent { match event { event::Event::Inertia(motion) => match motion { event::InertialEvent::Accelerometer(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), event::InertialEvent::Gyro(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Gyro), + Capability::Gyroscope(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), }, @@ -86,6 +86,6 @@ fn translate_event(event: event::Event) -> NativeEvent { /// List of all capabilities that the Legion Go driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), - Capability::Gamepad(Gamepad::Gyro), + Capability::Accelerometer(Source::Center), + Capability::Gyroscope(Source::Center), ]; diff --git a/src/input/source/hidraw/legos_xinput.rs b/src/input/source/hidraw/legos_xinput.rs index 7fbefc66..f13ece3b 100644 --- a/src/input/source/hidraw/legos_xinput.rs +++ b/src/input/source/hidraw/legos_xinput.rs @@ -374,7 +374,6 @@ fn translate_event(event: event::Event) -> NativeEvent { /// List of all capabilities that the Legion Go driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -397,7 +396,6 @@ pub const CAPABILITIES: &[Capability] = &[ Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), ]; diff --git a/src/input/source/hidraw/steam_deck.rs b/src/input/source/hidraw/steam_deck.rs index ee52077f..9514c5f2 100644 --- a/src/input/source/hidraw/steam_deck.rs +++ b/src/input/source/hidraw/steam_deck.rs @@ -13,21 +13,16 @@ use packed_struct::PackedStruct; use crate::{ drivers::{ dualsense::hid_report::SetStatePackedOutputData, - steam_deck::{ - self, - driver::{Driver, ACCEL_SCALE}, - hid_report::LIZARD_SLEEP_SEC, - }, + steam_deck::{self, driver::Driver, hid_report::LIZARD_SLEEP_SEC}, }, input::{ capability::{ - Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Touch, TouchButton, - Touchpad, + Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source, Touch, + TouchButton, Touchpad, }, event::{ native::NativeEvent, - value::InputValue, - value::{normalize_signed_value, normalize_unsigned_value}, + value::{normalize_signed_value, normalize_unsigned_value, InputValue}, }, output_capability::{Haptic, OutputCapability}, output_event::OutputEvent, @@ -529,20 +524,20 @@ fn translate_event(event: steam_deck::event::Event) -> NativeEvent { ), }, steam_deck::event::Event::Accelerometer(accel) => match accel { - steam_deck::event::AccelerometerEvent::Accelerometer(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Accelerometer), + steam_deck::event::IntertialEvent::Accelerometer(value) => NativeEvent::new( + Capability::Accelerometer(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64 * ACCEL_SCALE), - y: Some(value.y as f64 * ACCEL_SCALE), - z: Some(value.z as f64 * ACCEL_SCALE), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), - steam_deck::event::AccelerometerEvent::Attitude(value) => NativeEvent::new( - Capability::Gamepad(Gamepad::Gyro), + steam_deck::event::IntertialEvent::Gyroscope(value) => NativeEvent::new( + Capability::Gyroscope(Source::Center), InputValue::Vector3 { - x: Some(value.x as f64), - y: Some(value.y as f64), - z: Some(value.z as f64), + x: Some(value.x), + y: Some(value.y), + z: Some(value.z), }, ), }, @@ -591,7 +586,7 @@ fn translate_event(event: steam_deck::event::Event) -> NativeEvent { /// List of all capabilities that the Steam Deck driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -618,9 +613,9 @@ pub const CAPABILITIES: &[Capability] = &[ Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Press))), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Touch))), Capability::Touchpad(Touchpad::LeftPad(Touch::Motion)), diff --git a/src/input/source/iio/accel_gyro_3d.rs b/src/input/source/iio/accel_gyro_3d.rs index 737dacaa..bd55fa25 100644 --- a/src/input/source/iio/accel_gyro_3d.rs +++ b/src/input/source/iio/accel_gyro_3d.rs @@ -2,7 +2,7 @@ use std::{collections::HashSet, error::Error, fmt::Debug}; use crate::{ config, - drivers::iio_imu::{self, driver::Driver, info::MountMatrix}, + drivers::iio_imu::{self, hid_sfh_driver::Driver, info::MountMatrix}, input::{ capability::{Capability, Source}, event::{native::NativeEvent, value::InputValue}, @@ -11,17 +11,16 @@ use crate::{ udev::device::UdevDevice, }; -// Scale from IIO SI units to Steam Deck UHID raw LSB. -// IIO channels report m/s² for accel and rad/s for gyro after applying scale: -// https://www.kernel.org/doc/Documentation/ABI/testing/sysfs-bus-iio -// UHID LSB constants from src/drivers/steam_deck/driver.rs. -const ACCEL_SCALE_FACTOR: f64 = 1632.6530612244898; // 1 / 0.0006125 (m/s² → UHID LSB) -const GYRO_SCALE_FACTOR: f64 = 916.7324722093172; // (180/π) / 0.0625 (rad/s → °/s → UHID LSB) - pub struct AccelGyro3dImu { driver: Driver, } +// Sensor fusion hub devices produce bogus scale factors that reduce the output by the below +// factors. These were determined using real world testing. Without adjustment, Accelerometer +// data is in cm/s^2 instead of m/s^2 and Gyroscope data is milliradians instead of radians. +const SFH_ACCEL_CCORECTION: f64 = 10.0; +const SFH_GYRO_CORRECTION: f64 = 32.768; + impl AccelGyro3dImu { /// Create a new Accel Gyro 3D source device with the given udev /// device information @@ -49,8 +48,7 @@ impl AccelGyro3dImu { let sample_rate = config.as_ref().and_then(|c| c.sample_rate); let id = device_info.sysname(); - let name = device_info.name(); - let driver = Driver::new(id, name, mount_matrix, sample_rate)?; + let driver = Driver::new(id, mount_matrix, sample_rate)?; Ok(Self { driver }) } @@ -75,14 +73,13 @@ impl SourceInputDevice for AccelGyro3dImu { } fn get_default_event_filter(&self) -> Result, InputError> { - let filtered_events = self.driver.get_default_event_filter(); - let filtered_events = match filtered_events { - Ok(events) => events, - Err(e) => { - return Err(format!("Failed to get default event filter: {:?}", e).into()); - } - }; - Ok(filtered_events) + match std::fs::read_to_string("/proc/modules") { + Ok(modules) if modules.contains("hid_lenovo_go") => Ok(HashSet::from([ + Capability::Accelerometer(Source::Center), + Capability::Gyroscope(Source::Center), + ])), + _ => Ok(HashSet::new()), + } } } @@ -109,18 +106,18 @@ fn translate_event(event: iio_imu::event::Event) -> NativeEvent { iio_imu::event::Event::Accelerometer(data) => { let cap = Capability::Accelerometer(Source::Center); let value = InputValue::Vector3 { - x: Some(data.roll * ACCEL_SCALE_FACTOR), - y: Some(data.pitch * ACCEL_SCALE_FACTOR), - z: Some(data.yaw * ACCEL_SCALE_FACTOR), + x: Some(data.roll * SFH_ACCEL_CCORECTION), + y: Some(data.pitch * SFH_ACCEL_CCORECTION), + z: Some(data.yaw * SFH_ACCEL_CCORECTION), }; NativeEvent::new(cap, value) } iio_imu::event::Event::Gyro(data) => { let cap = Capability::Gyroscope(Source::Center); let value = InputValue::Vector3 { - x: Some(data.roll * GYRO_SCALE_FACTOR), - y: Some(data.pitch * GYRO_SCALE_FACTOR), - z: Some(data.yaw * GYRO_SCALE_FACTOR), + x: Some(data.roll * SFH_GYRO_CORRECTION), + y: Some(data.pitch * SFH_GYRO_CORRECTION), + z: Some(data.yaw * SFH_GYRO_CORRECTION), }; NativeEvent::new(cap, value) } diff --git a/src/input/source/iio/bmi_imu.rs b/src/input/source/iio/bmi_imu.rs index 329c675f..2b2a8264 100644 --- a/src/input/source/iio/bmi_imu.rs +++ b/src/input/source/iio/bmi_imu.rs @@ -1,10 +1,10 @@ -use std::{collections::HashSet, error::Error, f64::consts::PI, fmt::Debug}; +use std::{collections::HashSet, error::Error, fmt::Debug}; use crate::{ config, - drivers::iio_imu::{self, driver::Driver, info::MountMatrix}, + drivers::iio_imu::{self, bmi_driver::Driver, info::MountMatrix}, input::{ - capability::{Capability, Gamepad}, + capability::{Capability, Source}, event::{native::NativeEvent, value::InputValue}, source::{InputError, SourceInputDevice, SourceOutputDevice}, }, @@ -42,8 +42,7 @@ impl BmiImu { let sample_rate = config.as_ref().and_then(|c| c.sample_rate); let id = device_info.sysname(); - let name = device_info.name(); - let driver = Driver::new(id, name, mount_matrix, sample_rate)?; + let driver = Driver::new(id, mount_matrix, sample_rate)?; Ok(Self { driver }) } @@ -68,14 +67,7 @@ impl SourceInputDevice for BmiImu { } fn get_default_event_filter(&self) -> Result, InputError> { - let filtered_events = self.driver.get_default_event_filter(); - let filtered_events = match filtered_events { - Ok(events) => events, - Err(e) => { - return Err(format!("Failed to get default event filter: {:?}", e).into()); - } - }; - Ok(filtered_events) + Ok(HashSet::new()) } } @@ -100,7 +92,7 @@ fn translate_events(events: Vec) -> Vec { fn translate_event(event: iio_imu::event::Event) -> NativeEvent { match event { iio_imu::event::Event::Accelerometer(data) => { - let cap = Capability::Gamepad(Gamepad::Accelerometer); + let cap = Capability::Accelerometer(Source::Center); let value = InputValue::Vector3 { x: Some(data.roll), y: Some(data.pitch), @@ -109,16 +101,11 @@ fn translate_event(event: iio_imu::event::Event) -> NativeEvent { NativeEvent::new(cap, value) } iio_imu::event::Event::Gyro(data) => { - // Translate gyro values into the expected units of degrees per sec - // We apply a 12x scale so the lowest (default) value feels like natural 1:1 motion. - // Adjusting the scale will increase the granularity of the motion by slowing - // incrementing closer to 2:1 motion. From testing this is the highest scale we can - // apply before noise is amplified to the point the gyro cannot calibrate. - let cap = Capability::Gamepad(Gamepad::Gyro); + let cap = Capability::Gyroscope(Source::Center); let value = InputValue::Vector3 { - x: Some(data.roll * (180.0 / PI) * 12.0), - y: Some(data.pitch * (180.0 / PI) * 12.0), - z: Some(data.yaw * (180.0 / PI) * 12.0), + x: Some(data.roll), + y: Some(data.pitch), + z: Some(data.yaw), }; NativeEvent::new(cap, value) } @@ -127,6 +114,6 @@ fn translate_event(event: iio_imu::event::Event) -> NativeEvent { /// List of all capabilities that the driver implements pub const CAPABILITIES: &[Capability] = &[ - Capability::Gamepad(Gamepad::Accelerometer), - Capability::Gamepad(Gamepad::Gyro), + Capability::Accelerometer(Source::Center), + Capability::Gyroscope(Source::Center), ]; diff --git a/src/input/target/dualsense.rs b/src/input/target/dualsense.rs index b9b24493..f395867e 100644 --- a/src/input/target/dualsense.rs +++ b/src/input/target/dualsense.rs @@ -4,20 +4,12 @@ //! https://github.com/NeroReflex/ROGueENEMY/ use std::{cmp::Ordering, error::Error, fmt::Debug, fs::File, time::Duration}; -use packed_struct::prelude::*; +use packed_struct::{prelude::*, types::bits::Bits}; use rand::Rng; use uhid_virt::{Bus, CreateParams, StreamError, UHIDDevice}; use crate::{ drivers::dualsense::{ - driver::{ - DS5_ACC_RES_PER_G, DS5_EDGE_NAME, DS5_EDGE_PID, DS5_EDGE_VERSION, DS5_EDGE_VID, - DS5_NAME, DS5_PID, DS5_TOUCHPAD_HEIGHT, DS5_TOUCHPAD_WIDTH, DS5_VERSION, DS5_VID, - FEATURE_REPORT_CALIBRATION, FEATURE_REPORT_FIRMWARE_INFO, FEATURE_REPORT_PAIRING_INFO, - OUTPUT_REPORT_BT, OUTPUT_REPORT_BT_SIZE, OUTPUT_REPORT_USB, - OUTPUT_REPORT_USB_SHORT_SIZE, OUTPUT_REPORT_USB_SIZE, STICK_X_MAX, STICK_X_MIN, - STICK_Y_MAX, STICK_Y_MIN, TRIGGER_MAX, - }, hid_report::{ Direction, PackedInputDataReport, USBPackedInputDataReport, UsbPackedOutputReport, UsbPackedOutputReportShort, @@ -25,11 +17,17 @@ use crate::{ report_descriptor::{ DS_BT_DESCRIPTOR, DS_EDGE_BT_DESCRIPTOR, DS_EDGE_USB_DESCRIPTOR, DS_USB_DESCRIPTOR, }, + DS5_EDGE_NAME, DS5_EDGE_PID, DS5_EDGE_VERSION, DS5_EDGE_VID, DS5_NAME, DS5_PID, + DS5_RADS_TO_GYRO, DS5_SI_TO_ACCEL, DS5_TOUCHPAD_HEIGHT, DS5_TOUCHPAD_WIDTH, DS5_VERSION, + DS5_VID, FEATURE_REPORT_CALIBRATION, FEATURE_REPORT_FIRMWARE_INFO, + FEATURE_REPORT_PAIRING_INFO, OUTPUT_REPORT_BT, OUTPUT_REPORT_BT_SIZE, OUTPUT_REPORT_USB, + OUTPUT_REPORT_USB_SHORT_SIZE, OUTPUT_REPORT_USB_SIZE, STICK_X_MAX, STICK_X_MIN, + STICK_Y_MAX, STICK_Y_MIN, TRIGGER_MAX, }, input::{ capability::{ - Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Touch, TouchButton, - Touchpad, + Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source, Touch, + TouchButton, Touchpad, }, composite_device::client::CompositeDeviceClient, event::{ @@ -544,32 +542,6 @@ impl DualSenseDevice { GamepadTrigger::RightTouchpadForce => (), GamepadTrigger::RightStickForce => (), }, - Gamepad::Accelerometer => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - state.accel_x = Integer::from_primitive(denormalize_accel_value(x)) - } - if let Some(y) = y { - state.accel_y = Integer::from_primitive(denormalize_accel_value(y)) - } - if let Some(z) = z { - state.accel_z = Integer::from_primitive(denormalize_accel_value(z)) - } - } - } - Gamepad::Gyro => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - state.pitch = Integer::from_primitive(denormalize_gyro_value(x)); - } - if let Some(y) = y { - state.yaw = Integer::from_primitive(denormalize_gyro_value(y)) - } - if let Some(z) = z { - state.roll = Integer::from_primitive(denormalize_gyro_value(z)) - } - } - } _ => (), }, //TODO: Remove RightPad when we add target profiles @@ -627,26 +599,26 @@ impl DualSenseDevice { Capability::Gyroscope(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - state.pitch = Integer::from_primitive(x as i16); + state.pitch = denormalize_gyro_value(x); } if let Some(y) = y { - state.yaw = Integer::from_primitive(y as i16); + state.yaw = denormalize_gyro_value(y); } if let Some(z) = z { - state.roll = Integer::from_primitive(z as i16); + state.roll = denormalize_gyro_value(z); } } } Capability::Accelerometer(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - state.accel_x = Integer::from_primitive(x as i16); + state.accel_x = denormalize_accel_value(x); } if let Some(y) = y { - state.accel_y = Integer::from_primitive(y as i16); + state.accel_y = denormalize_accel_value(y); } if let Some(z) = z { - state.accel_z = Integer::from_primitive(z as i16); + state.accel_z = denormalize_accel_value(z); } } } @@ -966,7 +938,7 @@ impl TargetInputDevice for DualSenseDevice { fn get_capabilities(&self) -> Result, InputError> { Ok(vec![ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -992,9 +964,9 @@ impl TargetInputDevice for DualSenseDevice { Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), Capability::Touchpad(Touchpad::CenterPad(Touch::Button(TouchButton::Press))), Capability::Touchpad(Touchpad::CenterPad(Touch::Button(TouchButton::Touch))), Capability::Touchpad(Touchpad::CenterPad(Touch::Motion)), @@ -1172,15 +1144,12 @@ impl Debug for DualSenseDevice { /// values are measured in units of meters per second. To denormalize /// the value, it needs to be converted into G units (by dividing by 9.8), /// then multiplying that value by the [DS5_ACC_RES_PER_G]. -fn denormalize_accel_value(value_meters_sec: f64) -> i16 { - let value_g = value_meters_sec / 9.8; - let value = value_g * DS5_ACC_RES_PER_G as f64; - value as i16 +fn denormalize_accel_value(value_meters_sec: f64) -> Integer> { + Integer::from_primitive((value_meters_sec * DS5_SI_TO_ACCEL) as i16) } /// DualSense gyro values are measured in units of degrees per second. -/// InputPlumber gyro values are also measured in degrees per second. -fn denormalize_gyro_value(value_degrees_sec: f64) -> i16 { - let value = value_degrees_sec; - value as i16 +/// InputPlumber gyro values are measured in radians per second. +fn denormalize_gyro_value(value_degrees_sec: f64) -> Integer> { + Integer::from_primitive((value_degrees_sec * DS5_RADS_TO_GYRO) as i16) } diff --git a/src/input/target/horipad_steam.rs b/src/input/target/horipad_steam.rs index 009d284e..9844116b 100644 --- a/src/input/target/horipad_steam.rs +++ b/src/input/target/horipad_steam.rs @@ -1,22 +1,22 @@ //! Emulates a Horipad Steam Controller as a target input device. use std::{cmp::Ordering, error::Error, fmt::Debug, fs::File, time::Duration}; -use packed_struct::prelude::*; +use packed_struct::{prelude::*, types::bits::Bits}; use uhid_virt::{Bus, CreateParams, StreamError, UHIDDevice}; use crate::{ drivers::horipad_steam::{ - driver::{JOY_AXIS_MAX, JOY_AXIS_MIN, PIDS, TRIGGER_AXIS_MAX, VID}, hid_report::{Direction, PackedInputDataReport}, report_descriptor::REPORT_DESCRIPTOR, + HORIPAD_RADS_TO_GYRO, HORIPAD_SI_TO_ACCEL, JOY_AXIS_MAX, JOY_AXIS_MIN, PIDS, + TRIGGER_AXIS_MAX, VID, }, input::{ - capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger}, + capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source}, composite_device::client::CompositeDeviceClient, event::{ native::{NativeEvent, ScheduledNativeEvent}, - value::InputValue, - value::{denormalize_signed_value_u8, denormalize_unsigned_value_u8}, + value::{denormalize_signed_value_u8, denormalize_unsigned_value_u8, InputValue}, }, output_capability::OutputCapability, output_event::OutputEvent, @@ -239,57 +239,31 @@ impl HoripadSteamDevice { GamepadTrigger::RightTouchpadForce => (), GamepadTrigger::RightStickForce => (), }, - Gamepad::Accelerometer => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(denormalize_accel_value(x)) - } - if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(denormalize_accel_value(y)) - } - if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(denormalize_accel_value(z)) - } - } - } - Gamepad::Gyro => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.pitch = Integer::from_primitive(denormalize_gyro_value(x)); - } - if let Some(y) = y { - self.state.yaw = Integer::from_primitive(denormalize_gyro_value(y)) - } - if let Some(z) = z { - self.state.roll = Integer::from_primitive(denormalize_gyro_value(z)) - } - } - } _ => (), }, Capability::Gyroscope(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.pitch = Integer::from_primitive(x as i16); + self.state.pitch = denormalize_gyro_value(x); } if let Some(y) = y { - self.state.yaw = Integer::from_primitive(y as i16); + self.state.yaw = denormalize_gyro_value(y); } if let Some(z) = z { - self.state.roll = Integer::from_primitive(z as i16); + self.state.roll = denormalize_gyro_value(z); } } } Capability::Accelerometer(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); + self.state.accel_x = denormalize_accel_value(x); } if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); + self.state.accel_y = denormalize_accel_value(y); } if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); + self.state.accel_z = denormalize_accel_value(z); } } } @@ -313,9 +287,7 @@ impl HoripadSteamDevice { report_number: u8, _report_type: uhid_virt::ReportType, ) -> Result<(), Box> { - log::debug!( - "Received GetReport request: id: {id}, report_number: {report_number}" - ); + log::debug!("Received GetReport request: id: {id}, report_number: {report_number}"); if let Err(e) = self.device.write_get_report_reply(id, 1, vec![]) { log::warn!("Failed to write get report reply: {:?}", e); return Err(e.to_string().into()); @@ -333,7 +305,7 @@ impl TargetInputDevice for HoripadSteamDevice { fn get_capabilities(&self) -> Result, InputError> { Ok(vec![ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -361,9 +333,9 @@ impl TargetInputDevice for HoripadSteamDevice { Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), ]) } @@ -526,16 +498,14 @@ impl Debug for HoripadSteamDevice { /// values are measured in units of meters per second. To denormalize /// the value, it needs to be converted into G units (by dividing by 9.8), /// then multiplying that value by the []. -fn denormalize_accel_value(value_meters_sec: f64) -> i16 { - let value = value_meters_sec; - value as i16 +fn denormalize_accel_value(value_meters_sec: f64) -> Integer> { + Integer::from_primitive((value_meters_sec * HORIPAD_SI_TO_ACCEL) as i16) } /// SDL negates all gyro axes when reading from this device (SDL_hidapi_steam_hori.c L329-331): /// imu_data[N] = -1.0f * LOAD16(data[...]) /// We invert here so that SDL produces the correct sign after its negation. /// https://github.com/libsdl-org/SDL/blob/main/src/joystick/hidapi/SDL_hidapi_steam_hori.c#L329-L331 -fn denormalize_gyro_value(value_degrees_sec: f64) -> i16 { - let value = -value_degrees_sec; - value as i16 +fn denormalize_gyro_value(value_degrees_sec: f64) -> Integer> { + Integer::from_primitive((value_degrees_sec * HORIPAD_RADS_TO_GYRO) as i16) } diff --git a/src/input/target/steam_deck.rs b/src/input/target/steam_deck.rs index 071be6a9..4f4cf1f3 100644 --- a/src/input/target/steam_deck.rs +++ b/src/input/target/steam_deck.rs @@ -1,5 +1,5 @@ use packed_struct::{ - types::{Integer, SizedInteger}, + types::{bits::Bits, Integer, SizedInteger}, PackedStruct, }; use std::{ @@ -31,12 +31,12 @@ use crate::{ STICK_X_MAX, STICK_X_MIN, STICK_Y_MAX, STICK_Y_MIN, TRIGG_MAX, }, report_descriptor::{CONTROLLER_DESCRIPTOR, KEYBOARD_DESCRIPTOR, MOUSE_DESCRIPTOR}, - ProductId, VID, + ProductId, DECK_RADS_TO_GYRO, DECK_SI_TO_ACCEL, VID, }, input::{ capability::{ - Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Touch, TouchButton, - Touchpad, + Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source, Touch, + TouchButton, Touchpad, }, composite_device::client::CompositeDeviceClient, event::{ @@ -685,32 +685,6 @@ impl SteamDeckDevice { } } }, - Gamepad::Accelerometer => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); - } - if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); - } - if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); - } - } - } - Gamepad::Gyro => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.pitch = Integer::from_primitive(x as i16); - } - if let Some(y) = y { - self.state.yaw = Integer::from_primitive(y as i16); - } - if let Some(z) = z { - self.state.roll = Integer::from_primitive(z as i16); - } - } - } _ => (), }, Capability::Touchpad(touch) => match touch { @@ -779,26 +753,26 @@ impl SteamDeckDevice { Capability::Gyroscope(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.pitch = Integer::from_primitive(x as i16); + self.state.pitch = denormalize_gyro_value(x); } if let Some(y) = y { - self.state.yaw = Integer::from_primitive(y as i16); + self.state.yaw = denormalize_gyro_value(y); } if let Some(z) = z { - self.state.roll = Integer::from_primitive(z as i16); + self.state.roll = denormalize_gyro_value(z); } } } Capability::Accelerometer(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); + self.state.accel_x = denormalize_accel_value(x); } if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); + self.state.accel_y = denormalize_accel_value(y); } if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); + self.state.accel_z = denormalize_accel_value(z); } } } @@ -960,7 +934,7 @@ impl TargetInputDevice for SteamDeckDevice { fn get_capabilities(&self) -> Result, InputError> { Ok(vec![ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -988,13 +962,13 @@ impl TargetInputDevice for SteamDeckDevice { Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftStickForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTouchpadForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightStickForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTouchpadForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Press))), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Touch))), Capability::Touchpad(Touchpad::LeftPad(Touch::Motion)), @@ -1137,3 +1111,11 @@ pub fn denormalize_unsigned_to_signed_value(normal_value: f64, min: f64, max: f6 let normal_value = (normal_value * 2.0) - 1.0; denormalize_signed_value_i16(normal_value, min, max) } + +fn denormalize_accel_value(value_meters_sec: f64) -> Integer> { + Integer::from_primitive((value_meters_sec * DECK_SI_TO_ACCEL) as i16) +} + +fn denormalize_gyro_value(value_degrees_sec: f64) -> Integer> { + Integer::from_primitive((value_degrees_sec * DECK_RADS_TO_GYRO) as i16) +} diff --git a/src/input/target/steam_deck_uhid.rs b/src/input/target/steam_deck_uhid.rs index 3bf3f2a6..5b8a75bf 100644 --- a/src/input/target/steam_deck_uhid.rs +++ b/src/input/target/steam_deck_uhid.rs @@ -9,7 +9,7 @@ use std::{ }; use packed_struct::{ - types::{Integer, SizedInteger}, + types::{bits::Bits, Integer, SizedInteger}, PackedStruct, }; use uhid_virt::{Bus, CreateParams, StreamError, UHIDDevice}; @@ -23,12 +23,12 @@ use crate::{ STICK_X_MAX, STICK_X_MIN, STICK_Y_MAX, STICK_Y_MIN, TRIGG_MAX, }, report_descriptor::CONTROLLER_DESCRIPTOR, - ProductId, VID, + ProductId, DECK_RADS_TO_GYRO, DECK_SI_TO_ACCEL, VID, }, input::{ capability::{ - Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Touch, TouchButton, - Touchpad, + Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source, Touch, + TouchButton, Touchpad, }, composite_device::client::CompositeDeviceClient, event::{ @@ -289,32 +289,6 @@ impl SteamDeckUhidDevice { } } }, - Gamepad::Accelerometer => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); - } - if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); - } - if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); - } - } - } - Gamepad::Gyro => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.pitch = Integer::from_primitive(x as i16); - } - if let Some(y) = y { - self.state.yaw = Integer::from_primitive(y as i16); - } - if let Some(z) = z { - self.state.roll = Integer::from_primitive(z as i16); - } - } - } _ => (), }, Capability::Touchpad(touch) => match touch { @@ -383,26 +357,26 @@ impl SteamDeckUhidDevice { Capability::Gyroscope(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.pitch = Integer::from_primitive(x as i16); + self.state.pitch = denormalize_gyro_value(x); } if let Some(y) = y { - self.state.yaw = Integer::from_primitive(y as i16); + self.state.yaw = denormalize_gyro_value(y); } if let Some(z) = z { - self.state.roll = Integer::from_primitive(z as i16); + self.state.roll = denormalize_gyro_value(z); } } } Capability::Accelerometer(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); + self.state.accel_x = denormalize_accel_value(x); } if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); + self.state.accel_y = denormalize_accel_value(y); } if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); + self.state.accel_z = denormalize_accel_value(z); } } } @@ -768,7 +742,7 @@ impl TargetInputDevice for SteamDeckUhidDevice { fn get_capabilities(&self) -> Result, InputError> { Ok(vec![ - Capability::Gamepad(Gamepad::Accelerometer), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::RightStick)), Capability::Gamepad(Gamepad::Button(GamepadButton::DPadDown)), @@ -796,13 +770,13 @@ impl TargetInputDevice for SteamDeckUhidDevice { Capability::Gamepad(Gamepad::Button(GamepadButton::South)), Capability::Gamepad(Gamepad::Button(GamepadButton::Start)), Capability::Gamepad(Gamepad::Button(GamepadButton::West)), - Capability::Gamepad(Gamepad::Gyro), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftStickForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTouchpadForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightStickForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTouchpadForce)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Press))), Capability::Touchpad(Touchpad::LeftPad(Touch::Button(TouchButton::Touch))), Capability::Touchpad(Touchpad::LeftPad(Touch::Motion)), @@ -989,3 +963,11 @@ impl Debug for SteamDeckUhidDevice { .finish() } } + +fn denormalize_accel_value(value_meters_sec: f64) -> Integer> { + Integer::from_primitive((value_meters_sec * DECK_SI_TO_ACCEL) as i16) +} + +fn denormalize_gyro_value(value_degrees_sec: f64) -> Integer> { + Integer::from_primitive((value_degrees_sec * DECK_RADS_TO_GYRO) as i16) +} diff --git a/src/input/target/ultimate2_wireless.rs b/src/input/target/ultimate2_wireless.rs index c9b6f083..a258f645 100644 --- a/src/input/target/ultimate2_wireless.rs +++ b/src/input/target/ultimate2_wireless.rs @@ -5,8 +5,8 @@ use std::{cmp::Ordering, error::Error, fmt::Debug, fs::File, time::Duration}; -use packed_struct::prelude::*; use packed_struct::types::SizedInteger; +use packed_struct::{prelude::*, types::bits::Bits}; use uhid_virt::{Bus, CreateParams, StreamError, UHIDDevice}; use crate::{ @@ -18,12 +18,11 @@ use crate::{ report_descriptor::REPORT_DESCRIPTOR, }, input::{ - capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger}, + capability::{Capability, Gamepad, GamepadAxis, GamepadButton, GamepadTrigger, Source}, composite_device::client::CompositeDeviceClient, event::{ native::{NativeEvent, ScheduledNativeEvent}, - value::InputValue, - value::{denormalize_signed_value_u8, denormalize_unsigned_value_u8}, + value::{denormalize_signed_value_u8, denormalize_unsigned_value_u8, InputValue}, }, output_capability::OutputCapability, output_event::OutputEvent, @@ -190,38 +189,6 @@ impl Ultimate2WirelessDevice { _ => (), }, - // Axis layout (x=pitch, y=yaw, z=roll): yaw/roll axes are swapped - // relative to SDL sGyro/sAccel naming; pitch is negated. - Gamepad::Accelerometer => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.accel_y = - Integer::from_primitive(denormalize_accel(x).wrapping_neg()); - } - if let Some(y) = y { - self.state.accel_z = Integer::from_primitive(denormalize_accel(y)); - } - if let Some(z) = z { - self.state.accel_x = - Integer::from_primitive(denormalize_accel(z).wrapping_neg()); - } - } - } - - Gamepad::Gyro => { - if let InputValue::Vector3 { x, y, z } = value { - if let Some(x) = x { - self.state.gyro_y = Integer::from_primitive((x as i16).wrapping_neg()); - } - if let Some(y) = y { - self.state.gyro_z = Integer::from_primitive(y as i16); - } - if let Some(z) = z { - self.state.gyro_x = Integer::from_primitive((z as i16).wrapping_neg()); - } - } - } - _ => (), }, @@ -241,13 +208,13 @@ impl Ultimate2WirelessDevice { Capability::Accelerometer(_) => { if let InputValue::Vector3 { x, y, z } = value { if let Some(x) = x { - self.state.accel_x = Integer::from_primitive(x as i16); + self.state.accel_x = denormalize_accel_value(x); } if let Some(y) = y { - self.state.accel_y = Integer::from_primitive(y as i16); + self.state.accel_y = denormalize_accel_value(y); } if let Some(z) = z { - self.state.accel_z = Integer::from_primitive(z as i16); + self.state.accel_z = denormalize_accel_value(z); } } } @@ -347,8 +314,7 @@ impl TargetInputDevice for Ultimate2WirelessDevice { fn get_capabilities(&self) -> Result, InputError> { Ok(vec![ - Capability::Gamepad(Gamepad::Accelerometer), - Capability::Gamepad(Gamepad::Gyro), + Capability::Accelerometer(Source::Center), Capability::Gamepad(Gamepad::Button(GamepadButton::QuickAccess)), Capability::Gamepad(Gamepad::Button(GamepadButton::Screenshot)), Capability::Gamepad(Gamepad::Axis(GamepadAxis::LeftStick)), @@ -376,6 +342,7 @@ impl TargetInputDevice for Ultimate2WirelessDevice { Capability::Gamepad(Gamepad::Button(GamepadButton::West)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::LeftTrigger)), Capability::Gamepad(Gamepad::Trigger(GamepadTrigger::RightTrigger)), + Capability::Gyroscope(Source::Center), ]) } @@ -469,7 +436,7 @@ impl Debug for Ultimate2WirelessDevice { } // m/s² → raw i16 (4096 units = 1G) -fn denormalize_accel(value_m_s2: f64) -> i16 { - let g = value_m_s2 / GRAVITY; - (g * ACCEL_SCALE).clamp(i16::MIN as f64, i16::MAX as f64) as i16 +fn denormalize_accel_value(value_meters_sec: f64) -> Integer> { + let g = value_meters_sec / GRAVITY; + Integer::from_primitive((g * ACCEL_SCALE).clamp(i16::MIN as f64, i16::MAX as f64) as i16) } diff --git a/src/input/target/unified_gamepad.rs b/src/input/target/unified_gamepad.rs index cc7adeb3..9576c44c 100644 --- a/src/input/target/unified_gamepad.rs +++ b/src/input/target/unified_gamepad.rs @@ -463,36 +463,6 @@ impl From for StateUpdate { Self { capability, value } } - Gamepad::Accelerometer => { - let value = match event.get_value() { - InputValue::Vector3 { x, y, z } => Int16Vector3Update { - x: x.map(|x| (x * ACCEL_SCALE_FACTOR) as i16), - y: y.map(|y| (y * ACCEL_SCALE_FACTOR) as i16), - z: z.map(|z| (z * ACCEL_SCALE_FACTOR) as i16), - }, - _ => { - return Self::default(); - } - }; - let value = ValueUpdate::Int16Vector3(value); - - Self { capability, value } - } - Gamepad::Gyro => { - let value = match event.get_value() { - InputValue::Vector3 { x, y, z } => Int16Vector3Update { - x: x.map(|x| (x * GYRO_SCALE_FACTOR) as i16), - y: y.map(|y| (y * GYRO_SCALE_FACTOR) as i16), - z: z.map(|z| (z * GYRO_SCALE_FACTOR) as i16), - }, - _ => { - return Self::default(); - } - }; - let value = ValueUpdate::Int16Vector3(value); - - Self { capability, value } - } Gamepad::Dial(_) => { let value = match event.get_value() { InputValue::Bool(n) => BoolUpdate { value: n }, @@ -780,8 +750,6 @@ impl From for InputCapability { GamepadTrigger::RightTouchpadForce => Self::GamepadTriggerRightTouchpadForce, GamepadTrigger::RightStickForce => Self::GamepadTriggerRightStickForce, }, - Gamepad::Accelerometer => Self::AccelerometerCenter, - Gamepad::Gyro => Self::GyroscopeCenter, Gamepad::Dial(dial) => match dial { GamepadDial::LeftStickDial => Self::GamepadDialLeft, GamepadDial::RightStickDial => Self::GamepadDialRight, @@ -1003,8 +971,6 @@ impl From for InputCapabilityInfo { Gamepad::Button(_) => Self::new(capability, ValueType::Bool), Gamepad::Axis(_) => Self::new(capability, ValueType::UInt16Vector2), Gamepad::Trigger(_) => Self::new(capability, ValueType::UInt8), - Gamepad::Accelerometer => Self::new(capability, ValueType::Int16Vector3), - Gamepad::Gyro => Self::new(capability, ValueType::Int16Vector3), Gamepad::Dial(_) => Self::new(capability, ValueType::Int8), }, Capability::Mouse(_) => Self::default(),