diff --git a/src/dbus/interface/source/iio_imu.rs b/src/dbus/interface/source/iio_imu.rs new file mode 100644 index 0000000..375dde4 --- /dev/null +++ b/src/dbus/interface/source/iio_imu.rs @@ -0,0 +1,312 @@ +use std::error::Error; + +use crate::iio::device::Device; +use tokio::sync::mpsc::Sender; +use zbus::{fdo, Connection}; +use zbus_macros::interface; + +use crate::input::source::{iio::get_dbus_path, SourceCommand}; + +/// DBusInterface exposing information about a HIDRaw device +pub struct SourceIioImuInterface { + info: Device, + tx: Sender, +} + +impl SourceIioImuInterface { + pub fn new(info: Device, tx: Sender) -> SourceIioImuInterface { + SourceIioImuInterface { info, tx } + } + + /// Creates a new instance of the source hidraw interface on DBus. Returns + /// a structure with information about the source device. + pub async fn listen_on_dbus( + conn: Connection, + info: Device, + tx: Sender, + ) -> Result<(), Box> { + let Some(id) = info.id.clone() else { + return Err("Failed to get ID of IIO device".into()); + }; + let path = get_dbus_path(id); + + let iface = SourceIioImuInterface::new(info, tx); + tokio::task::spawn(async move { + log::debug!("Starting dbus interface: {path}"); + let result = conn.object_server().at(path.clone(), iface).await; + if let Err(e) = result { + log::debug!("Failed to start dbus interface {path}: {e:?}"); + } else { + log::debug!("Started dbus interface: {path}"); + } + }); + Ok(()) + } +} + +#[interface(name = "org.shadowblip.Input.Source.IIOIMUDevice")] +impl SourceIioImuInterface { + #[zbus(property)] + async fn accel_sample_rate(&self) -> fdo::Result { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetSampleRate("accel".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn accel_sample_rates_avail(&self) -> fdo::Result> { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetSampleRatesAvail("accel".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn angvel_sample_rate(&self) -> fdo::Result { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetSampleRate("gyro".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn angvel_sample_rates_avail(&self) -> fdo::Result> { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetSampleRatesAvail("gyro".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn set_accel_sample_rate(&self, sample_rate: f64) -> zbus::Result<()> { + let (tx, rx) = std::sync::mpsc::channel(); + + if let Err(e) = self + .tx + .send(SourceCommand::SetSampleRate( + "accel".to_string(), + sample_rate, + tx, + )) + .await + { + return Err(zbus::Error::Failure(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(zbus::Error::Failure( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(zbus::Error::Failure(e.to_string())), + } + } + + #[zbus(property)] + async fn set_angvel_sample_rate(&self, sample_rate: f64) -> zbus::Result<()> { + let (tx, rx) = std::sync::mpsc::channel(); + + if let Err(e) = self + .tx + .send(SourceCommand::SetSampleRate( + "gyro".to_string(), + sample_rate, + tx, + )) + .await + { + return Err(zbus::Error::Failure(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(zbus::Error::Failure( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(zbus::Error::Failure(e.to_string())), + } + } + // + #[zbus(property)] + async fn accel_scale(&self) -> fdo::Result { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetScale("accel".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn accel_scales_avail(&self) -> fdo::Result> { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetScalesAvail("accel".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn angvel_scale(&self) -> fdo::Result { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetScale("gyro".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn angvel_scales_avail(&self) -> fdo::Result> { + let (tx, rx) = std::sync::mpsc::channel(); + if let Err(e) = self + .tx + .send(SourceCommand::GetScalesAvail("gyro".to_string(), tx)) + .await + { + return Err(fdo::Error::Failed(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(fdo::Error::Failed( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(fdo::Error::Failed(e.to_string())), + } + } + + #[zbus(property)] + async fn set_accel_scale(&self, scale: f64) -> zbus::Result<()> { + let (tx, rx) = std::sync::mpsc::channel(); + + if let Err(e) = self + .tx + .send(SourceCommand::SetScale("accel".to_string(), scale, tx)) + .await + { + return Err(zbus::Error::Failure(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(zbus::Error::Failure( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(zbus::Error::Failure(e.to_string())), + } + } + + #[zbus(property)] + async fn set_angvel_scale(&self, scale: f64) -> zbus::Result<()> { + let (tx, rx) = std::sync::mpsc::channel(); + + if let Err(e) = self + .tx + .send(SourceCommand::SetScale("gyro".to_string(), scale, tx)) + .await + { + return Err(zbus::Error::Failure(e.to_string())); + } + let Ok(response) = rx.recv() else { + return Err(zbus::Error::Failure( + "Channel closed with no response.".to_string(), + )); + }; + match response { + Ok(result) => Ok(result), + Err(e) => Err(zbus::Error::Failure(e.to_string())), + } + } +} diff --git a/src/dbus/interface/source/mod.rs b/src/dbus/interface/source/mod.rs index cf81dcb..4b7f32d 100644 --- a/src/dbus/interface/source/mod.rs +++ b/src/dbus/interface/source/mod.rs @@ -1,2 +1,3 @@ pub mod evdev; pub mod hidraw; +pub mod iio_imu; diff --git a/src/drivers/iio_imu/driver.rs b/src/drivers/iio_imu/driver.rs index 4ad0950..e6f87e1 100644 --- a/src/drivers/iio_imu/driver.rs +++ b/src/drivers/iio_imu/driver.rs @@ -1,4 +1,4 @@ -use std::{collections::HashMap, error::Error}; +use std::{collections::HashMap, error::Error, time::Duration}; use industrial_io::{Channel, ChannelType, Device}; @@ -16,6 +16,7 @@ pub struct Driver { accel_info: HashMap, gyro: HashMap, gyro_info: HashMap, + pub sample_delay: Duration, } impl Driver { @@ -54,29 +55,38 @@ impl Driver { // 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::debug!("Found device attribute: {:?}", attr) + log::trace!("Found device attribute: {:?}", attr) } // Log all found channels for channel in device.channels() { - log::debug!("Found channel: {:?} {:?}", channel.id(), channel.name()); - log::debug!(" Is output: {}", channel.is_output()); - log::debug!(" Is scan element: {}", channel.is_scan_element()); + 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::debug!(" Found attribute: {:?}", attr); + log::trace!(" Found attribute: {:?}", attr); } } + // Calculate the initial sample delay + Ok(Self { mount_matrix, accel, accel_info, gyro, gyro_info, + sample_delay: Duration::from_micros(2500), //400Hz }) } @@ -106,7 +116,7 @@ impl Driver { let Some(info) = self.accel_info.get(id) else { continue; }; - let data = channel.attr_read::("raw")?; + let data = channel.attr_read_int("raw")?; // processed_value = (raw + offset) * scale let value = (data + info.offset) as f64 * info.scale; @@ -134,7 +144,7 @@ impl Driver { let Some(info) = self.gyro_info.get(id) else { continue; }; - let data = channel.attr_read::("raw")?; + let data = channel.attr_read_int("raw")?; // processed_value = (raw + offset) * scale let value = (data + info.offset) as f64 * info.scale; @@ -177,6 +187,280 @@ impl Driver { value.y = mxy * x + myy * y + mzy * z; value.z = mxz * x + myz * y + mzz * z; } + + /// Calculates the duration in seconds that this device should sleep for before polling for + /// events again. Uses the fastest frequency set between the currently set accelerometer and + /// gyroscope sample rates. Called automatically when the sample_rate is changed. + pub fn calculate_sample_delay(&self) -> Result> { + let accel_rate = self.get_sample_rate("accel").unwrap_or(1.0); + let gyro_rate = self.get_sample_rate("gyro").unwrap_or(1.0); + let sample_delay = 1.0 / accel_rate.max(gyro_rate); + log::debug!("Updated sample delay is: {sample_delay} seconds."); + + Ok(Duration::from_secs_f64(sample_delay)) + } + + /// Returns the currently set sample rate from the X axis of the given input source, either + /// "accel" or "gyro". + pub fn get_sample_rate(&self, imu_type: &str) -> Result> { + match imu_type { + "accel" => { + let Some(info) = self.accel_info.get("accel_x") else { + return Err(format!("Unable to get sample rate for IMU type {imu_type}").into()); + }; + + Ok(info.sample_rate) + } + "gyro" => { + let Some(info) = self.gyro_info.get("anglvel_x") else { + return Err(format!("Unable to get sample rate for IMU type {imu_type}").into()); + }; + //log::debug!("sample rate found: {:?}", info.sample_rate); + + Ok(info.sample_rate) + } + _ => Err(format!("{imu_type} is not a valid imu type.").into()), + } + } + + /// Returns the available sample rates for the given input source, either "accel" or "gyro". + pub fn get_sample_rates_avail(&self, imu_type: &str) -> Result, Box> { + match imu_type { + "accel" => { + let Some(info) = self.accel_info.get("accel_x") else { + return Err(format!("Unable to get sample rate for IMU type {imu_type}").into()); + }; + + Ok(info.sample_rates_avail.clone()) + } + "gyro" => { + let Some(info) = self.gyro_info.get("anglvel_x") else { + return Err(format!("Unable to get sample rate for IMU type {imu_type}").into()); + }; + //log::debug!("sample rates avail found: {:?}", info.sample_rates_avail); + + Ok(info.sample_rates_avail.clone()) + } + _ => Err(format!("{imu_type} is not a valid imu type.").into()), + } + } + + /// Sets the given input source, either "accel" or "gyro", to the given rate. Returns an error + /// if the given sample rate is not in the list of valid sample rates. + pub fn set_sample_rate(&mut self, imu_type: &str, rate: f64) -> Result<(), Box> { + match imu_type { + "accel" => { + for (id, channel) in &self.accel { + let Some(info) = self.accel_info.get_mut(id) else { + return Err(format!( + "Unable to get channel info for {imu_type} channel {id}" + ) + .into()); + }; + + if !info.sample_rates_avail.contains(&rate) { + return Err(format!( + "Unable to set sample rate to {rate}, frequency not supported." + ) + .into()); + } + + if info.sample_rate == rate { + log::debug!("IMU {imu_type} Channel {channel:?} already set to {rate:?}. Nothing to do."); + continue; + }; + + let result = channel.attr_write_float("sampling_frequency", rate); + match result { + Ok(_) => { + log::debug!("IMU {imu_type} Channel {channel:?} set to {rate:?}."); + info.sample_rate = rate; + } + + Err(e) => { + return Err(format!( + "Unable to set sample rate for channel {id}, got error {e}" + ) + .into()) + } + } + } + } + "gyro" => { + for (id, channel) in &self.gyro { + let Some(info) = self.gyro_info.get_mut(id) else { + return Err(format!( + "Unable to get channel info for {imu_type} channel {id}" + ) + .into()); + }; + + if !info.sample_rates_avail.contains(&rate) { + return Err(format!( + "Unable to set sample rate to {rate}, frequency not supported." + ) + .into()); + } + + if info.sample_rate == rate { + log::debug!("IMU {imu_type} Channel {channel:?} already set to {rate:?}. Nothing to do."); + continue; + }; + + let result = channel.attr_write_float("sampling_frequency", rate); + match result { + Ok(_) => { + log::debug!("IMU {imu_type} Channel {channel:?} set to {rate:?}."); + info.sample_rate = rate; + } + + Err(e) => { + return Err(format!( + "Unable to set sample rate for channel {id}, got error {e}" + ) + .into()) + } + } + } + } + _ => { + return Err(format!("{imu_type} is not a valid imu type.").into()); + } + } + self.sample_delay = self.calculate_sample_delay()?; + Ok(()) + } + + /// Returns the currently set cale from the X axis of the given input source, either "accel" or "gyro". + pub fn get_scale(&self, imu_type: &str) -> Result> { + match imu_type { + "accel" => { + let Some(info) = self.accel_info.get("accel_x") else { + return Err(format!("Unable to get scale for IMU type {imu_type}.").into()); + }; + + Ok(info.scale) + } + "gyro" => { + let Some(info) = self.gyro_info.get("anglvel_x") else { + return Err( + format!("Unable to get scale available for IMU type {imu_type}.").into(), + ); + }; + + Ok(info.scale) + } + _ => Err(format!("{imu_type} is not a valid imu type.").into()), + } + } + + /// Returns the available scales for the given input source, either "accel" or "gyro". + pub fn get_scales_avail(&self, imu_type: &str) -> Result, Box> { + match imu_type { + "accel" => { + let Some(info) = self.accel_info.get("accel_x") else { + return Err(format!("Unable to scale for IMU type {imu_type}.").into()); + }; + + Ok(info.scales_avail.clone()) + } + "gyro" => { + let Some(info) = self.gyro_info.get("anglvel_x") else { + return Err( + format!("Unable to get scales available for IMU type {imu_type}.").into(), + ); + }; + + Ok(info.scales_avail.clone()) + } + _ => Err(format!("{imu_type} is not a valid imu type.").into()), + } + } + + /// Sets the given input source, either "accel" or "gyro", to the given scale. Returns an error + /// if the given scale is not in the list of valid sample rates. + pub fn set_scale(&mut self, imu_type: &str, scale: f64) -> Result<(), Box> { + match imu_type { + "accel" => { + for (id, channel) in &self.accel { + let Some(info) = self.accel_info.get_mut(id) else { + return Err(format!( + "Unable to get channel info for {imu_type} channel {id}." + ) + .into()); + }; + + if !info.scales_avail.contains(&scale) { + return Err(format!( + "Unable to set scale to {scale}, scale not supported." + ) + .into()); + } + + if info.scale == scale { + log::debug!("IMU {imu_type} Channel {channel:?} already set to {scale:?}. Nothing to do."); + continue; + }; + + let result = channel.attr_write_float("scale", scale); + match result { + Ok(_) => { + log::debug!("IMU {imu_type} Channel {channel:?} set to {scale:?}."); + info.scale = scale; + } + + Err(e) => { + return Err(format!( + "Unable to set scale rate for channel {id}, got error {e}" + ) + .into()) + } + } + } + } + "gyro" => { + for (id, channel) in &self.gyro { + let Some(info) = self.gyro_info.get_mut(id) else { + return Err(format!( + "Unable to get channel info for {imu_type} channel {id}." + ) + .into()); + }; + + if !info.scales_avail.contains(&scale) { + return Err(format!( + "Unable to set scale to {scale}, scale not supported." + ) + .into()); + }; + + if info.scale == scale { + log::debug!("IMU {imu_type} Channel {channel:?} already set to {scale:?}. Nothing to do."); + continue; + }; + + let result = channel.attr_write_float("scale", scale); + match result { + Ok(_) => { + log::debug!("IMU {imu_type} Channel {channel:?} set to {scale:?}."); + info.scale = scale; + } + + Err(e) => { + return Err(format!( + "Unable to set scale for channel {id}, got error {e}." + ) + .into()) + } + } + } + } + _ => { + return Err(format!("{imu_type} is not a valid imu type.").into()); + } + } + Ok(()) + } } /// Returns all channels and channel information from the given device matching @@ -197,9 +481,45 @@ fn get_channels_with_type( }; 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::("scale") { + let scale = match channel.attr_read_float("scale") { Ok(v) => v, Err(e) => { log::warn!("Unable to read scale for channel {id}: {:?}", e); @@ -207,16 +527,28 @@ fn get_channels_with_type( } }; - // Get the offset of the axis - let offset = match channel.attr_read::("offset") { - Ok(v) => v, + 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::debug!("Unable to read offset for channel {id}: {:?}", e); - 0 + log::warn!("Unable to read available scales for channel {id}: {:?}", e); + vec![1.0] } }; - let info = AxisInfo { scale, offset }; + let info = AxisInfo { + offset, + sample_rate, + sample_rates_avail, + scale, + scales_avail, + }; channel_info.insert(id.clone(), info); channels.insert(id, channel); }); diff --git a/src/drivers/iio_imu/info.rs b/src/drivers/iio_imu/info.rs index 0202aa9..2fcd66f 100644 --- a/src/drivers/iio_imu/info.rs +++ b/src/drivers/iio_imu/info.rs @@ -1,4 +1,4 @@ -use std::{error::Error, fmt}; +use std::{collections::HashSet, error::Error, fmt}; /// The [MountMatrix] is used to define how sensors are oriented inside a device /// https://github.com/torvalds/linux/blob/master/Documentation/devicetree/bindings/iio/mount-matrix.txt @@ -74,8 +74,11 @@ impl fmt::Display for MountMatrix { /// processed_value = (raw + offset) * scale #[derive(Clone, Debug)] pub struct AxisInfo { - pub scale: f64, pub offset: i64, + pub sample_rate: f64, + pub sample_rates_avail: Vec, + pub scale: f64, + pub scales_avail: Vec, } /// Scale and offset information for all axes diff --git a/src/input/composite_device/mod.rs b/src/input/composite_device/mod.rs index 6589664..54a40d2 100644 --- a/src/input/composite_device/mod.rs +++ b/src/input/composite_device/mod.rs @@ -12,7 +12,9 @@ use crate::{ config::{ CapabilityMap, CapabilityMapping, CompositeDeviceConfig, DeviceProfile, ProfileMapping, }, - dbus::interface::composite_device::CompositeDeviceInterface, + dbus::interface::{ + composite_device::CompositeDeviceInterface, source::iio_imu::SourceIioImuInterface, + }, input::{ capability::{Capability, Gamepad, GamepadButton, Mouse}, event::{ @@ -546,6 +548,18 @@ impl CompositeDevice { let source_tx = source_device.transmitter(); self.source_devices.insert(device_id.clone(), source_tx); let tx = self.tx.clone(); + + // Add the IIO IMU Dbus interface. We do this here because it needs the source + // device transmitter and this is the only place we can refrence it at the moment. + if let SourceDevice::IIODevice(ref device) = source_device { + SourceIioImuInterface::listen_on_dbus( + self.conn.clone(), + device.get_info(), + device.transmitter(), + ) + .await?; + } + self.source_device_tasks.spawn(async move { if let Err(e) = source_device.run().await { log::error!("Failed running device: {:?}", e); @@ -1278,6 +1292,7 @@ impl CompositeDevice { /// Executed whenever a source device is removed from this [CompositeDevice] async fn on_source_device_removed(&mut self, id: String) -> Result<(), Box> { + // Handle evdev if id.starts_with("evdev://") { let name = id.strip_prefix("evdev://").unwrap(); let path = format!("/dev/input/{}", name); @@ -1291,7 +1306,8 @@ impl CompositeDevice { }; self.source_devices_blocked.remove(&id); } - if id.starts_with("hidraw://") { + // Handle HIDRAW + else if id.starts_with("hidraw://") { let name = id.strip_prefix("hidraw://").unwrap(); let path = format!("/dev/{}", name); @@ -1304,6 +1320,20 @@ impl CompositeDevice { }; self.source_devices_blocked.remove(&id); } + // Handle IIO + else if id.starts_with("iio://") { + let name = id.strip_prefix("iio://").unwrap(); + let path = format!("/sys/bus/iio/devices/{}", name); + + if let Some(idx) = self.source_device_paths.iter().position(|str| str == &path) { + self.source_device_paths.remove(idx); + }; + + if let Some(idx) = self.source_devices_used.iter().position(|str| str == &id) { + self.source_devices_used.remove(idx); + }; + self.source_devices_blocked.remove(&id); + } // Signal to DBus that source devices have changed self.signal_sources_changed().await; diff --git a/src/input/manager.rs b/src/input/manager.rs index 3e24559..fed7fac 100644 --- a/src/input/manager.rs +++ b/src/input/manager.rs @@ -19,6 +19,7 @@ use crate::dbus::interface::composite_device::CompositeDeviceInterface; use crate::dbus::interface::manager::ManagerInterface; use crate::dbus::interface::source::evdev::SourceEventDeviceInterface; use crate::dbus::interface::source::hidraw::SourceHIDRawInterface; +use crate::dbus::interface::source::iio_imu::SourceIioImuInterface; use crate::dmi::data::DMIData; use crate::dmi::get_cpu_info; use crate::dmi::get_dmi_data; @@ -368,7 +369,7 @@ impl Manager { }; // Create a composite device to manage these devices - log::info!("Found matching source devices: {:?}", config.name); + log::info!("Found matching source device for: {:?}", config.name); let config = config.clone(); let device = CompositeDevice::new( self.dbus.clone(), @@ -735,11 +736,11 @@ impl Manager { let Some(config) = self.used_configs.get(composite_device) else { continue; }; - log::debug!("Checking existing config {:?} for device", config.name); + log::trace!("Checking existing config {:?} for device", config.name); let source_devices = config.source_devices.clone(); match device_info.clone() { SourceDeviceInfo::EvdevDeviceInfo(info) => { - log::debug!("Checking if existing composite device is missing event device"); + log::trace!("Checking if existing composite device is missing event device"); for source_device in source_devices { if source_device.evdev.is_none() { continue; @@ -761,12 +762,12 @@ impl Manager { } if let Some(unique) = source_device.clone().unique { if unique { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } // Default to being unique } else { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } } @@ -802,7 +803,7 @@ impl Manager { } } SourceDeviceInfo::HIDRawDeviceInfo(info) => { - log::debug!("Checking if existing composite device is missing hidraw device"); + log::trace!("Checking if existing composite device is missing hidraw device"); for source_device in source_devices { if source_device.hidraw.is_none() { continue; @@ -825,11 +826,11 @@ impl Manager { } if let Some(unique) = source_device.clone().unique { if unique { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } } else { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } } @@ -865,7 +866,7 @@ impl Manager { } } SourceDeviceInfo::IIODeviceInfo(info) => { - log::debug!("Checking if existing composite device is missing hidraw device"); + log::trace!("Checking if existing composite device is missing hidraw device"); for source_device in source_devices { if source_device.iio.is_none() { continue; @@ -887,11 +888,11 @@ impl Manager { } if let Some(unique) = source_device.clone().unique { if unique { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } } else { - log::debug!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); + log::trace!("Found unique device {:?}, not adding to composite device {}", source_device, composite_device); break 'start; } } @@ -927,7 +928,7 @@ impl Manager { } } } - log::debug!("Device does not match existing device: {:?}", config.name); + log::trace!("Device does not match existing device: {:?}", config.name); } log::debug!("No existing composite device matches device."); @@ -936,11 +937,11 @@ impl Manager { let configs = self.load_device_configs().await; log::debug!("Checking unused configs"); for config in configs { - log::debug!("Checking config {:?} for device", config.name); + log::trace!("Checking config {:?} for device", config.name); // Check to see if this configuration matches the system if !config.has_valid_matches(&self.dmi_data, &self.cpu_info) { - log::debug!("Configuration does not match system"); + log::trace!("Configuration does not match system"); continue; } @@ -1055,7 +1056,7 @@ impl Manager { } } } - log::debug!("Device does not match config: {:?}", config.name); + log::trace!("Device does not match config: {:?}", config.name); } log::debug!("No unused configs found for device."); @@ -1266,7 +1267,24 @@ impl Manager { async fn on_iio_removed(&mut self, id: String) -> Result<(), Box> { log::debug!("IIO device removed: {}", id); let id = format!("iio://{}", id); - self.on_source_device_removed(id).await?; + self.on_source_device_removed(id.clone()).await?; + + // Remove the DBus interface + // We do this here because we connect in the CompostiteDevice. + let conn = self.dbus.clone(); + let path = crate::input::source::iio::get_dbus_path(id); + tokio::task::spawn(async move { + log::debug!("Stopping dbus interface for {path}"); + let result = conn + .object_server() + .remove::(path.clone()) + .await; + if let Err(e) = result { + log::error!("Failed to stop dbus interface {path}: {e:?}"); + } else { + log::debug!("Stopped dbus interface for {path}"); + } + }); Ok(()) } @@ -1520,7 +1538,7 @@ impl Manager { for path in paths { let files = fs::read_dir(path); if files.is_err() { - log::debug!("Failed to load directory {}: {}", path, files.unwrap_err()); + log::warn!("Failed to load directory {}: {}", path, files.unwrap_err()); continue; } let mut files: Vec<_> = files.unwrap().map(|r| r.unwrap()).collect(); @@ -1537,10 +1555,10 @@ impl Manager { } // Try to load the composite device profile - log::debug!("Found file: {}", file.path().display()); + log::trace!("Found file: {}", file.path().display()); let mapping = CapabilityMap::from_yaml_file(file.path().display().to_string()); if mapping.is_err() { - log::debug!( + log::warn!( "Failed to parse capability mapping: {}", mapping.unwrap_err() ); @@ -1568,10 +1586,10 @@ impl Manager { // Look for composite device profiles in all known locations for path in paths { - log::debug!("Checking {path} for composite device configs"); + log::trace!("Checking {path} for composite device configs"); let files = fs::read_dir(path); if files.is_err() { - log::debug!("Failed to load directory {}: {}", path, files.unwrap_err()); + log::warn!("Failed to load directory {}: {}", path, files.unwrap_err()); continue; } let mut files: Vec<_> = files.unwrap().map(|r| r.unwrap()).collect(); @@ -1588,11 +1606,11 @@ impl Manager { } // Try to load the composite device profile - log::debug!("Found file: {}", file.path().display()); + log::trace!("Found file: {}", file.path().display()); let device = CompositeDeviceConfig::from_yaml_file(file.path().display().to_string()); if device.is_err() { - log::debug!( + log::warn!( "Failed to parse composite device config: {}", device.unwrap_err() ); diff --git a/src/input/source/evdev.rs b/src/input/source/evdev.rs index 24ade2b..18c3f06 100644 --- a/src/input/source/evdev.rs +++ b/src/input/source/evdev.rs @@ -216,6 +216,12 @@ impl EventDevice { } } SourceCommand::Stop => return Err("Device stopped".into()), + SourceCommand::GetSampleRate(_, _) => (), + SourceCommand::GetSampleRatesAvail(_, _) => (), + SourceCommand::SetSampleRate(_, _, _) => (), + SourceCommand::GetScale(_, _) => (), + SourceCommand::GetScalesAvail(_, _) => (), + SourceCommand::SetScale(_, _, _) => (), }, Err(e) => match e { TryRecvError::Empty => return Ok(()), diff --git a/src/input/source/hidraw/steam_deck.rs b/src/input/source/hidraw/steam_deck.rs index 8f485f1..c1b5b9f 100644 --- a/src/input/source/hidraw/steam_deck.rs +++ b/src/input/source/hidraw/steam_deck.rs @@ -188,6 +188,12 @@ impl DeckOutput { } } SourceCommand::Stop => return Err("Device stopped".into()), + SourceCommand::GetSampleRate(_, _) => (), + SourceCommand::GetSampleRatesAvail(_, _) => (), + SourceCommand::SetSampleRate(_, _, _) => (), + SourceCommand::GetScale(_, _) => (), + SourceCommand::GetScalesAvail(_, _) => (), + SourceCommand::SetScale(_, _, _) => (), }, Err(e) => match e { TryRecvError::Empty => return Ok(()), diff --git a/src/input/source/iio.rs b/src/input/source/iio.rs index 251f4bc..01ccf37 100644 --- a/src/input/source/iio.rs +++ b/src/input/source/iio.rs @@ -94,6 +94,10 @@ impl IIODevice { Ok(vec![]) } + pub fn get_info(&self) -> Device { + self.info.clone() + } + /// Returns a unique identifier for the source device. pub fn get_id(&self) -> String { let name = self.info.id.clone().unwrap_or_default(); diff --git a/src/input/source/iio/iio_imu.rs b/src/input/source/iio/iio_imu.rs index 9f4172a..227f0a7 100644 --- a/src/input/source/iio/iio_imu.rs +++ b/src/input/source/iio/iio_imu.rs @@ -1,6 +1,7 @@ use core::time; -use std::{error::Error, f64::consts::PI, thread}; +use std::{any::Any, error::Error, f64::consts::PI, thread}; +use nix::libc::ETIMEDOUT; use tokio::sync::mpsc::{self, error::TryRecvError}; use crate::{ @@ -73,9 +74,9 @@ impl IMU { // data. let task = tokio::task::spawn_blocking(move || -> Result<(), Box> { - let driver = Driver::new(id, name, mount_matrix)?; + let mut driver = Driver::new(id, name, mount_matrix)?; loop { - receive_commands(&mut rx)?; + receive_commands(&mut rx, &mut driver)?; let events = driver.poll()?; let native_events = translate_events(events); for event in native_events { @@ -89,10 +90,8 @@ impl IMU { Event::Native(event), ))?; } - // Sleep between each poll iteration - let duration = time::Duration::from_micros(250); - thread::sleep(duration); + thread::sleep(driver.sample_delay); } }); @@ -127,11 +126,15 @@ fn translate_event(event: iio_imu::event::Event) -> NativeEvent { } 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 value = InputValue::Vector3 { - x: Some(data.x * (180.0 / PI)), - y: Some(data.y * (180.0 / PI)), - z: Some(data.z * (180.0 / PI)), + x: Some(data.x * (180.0 / PI) * 12.0), + y: Some(data.y * (180.0 / PI) * 12.0), + z: Some(data.z * (180.0 / PI) * 12.0), }; NativeEvent::new(cap, value) } @@ -142,16 +145,79 @@ fn translate_event(event: iio_imu::event::Event) -> NativeEvent { /// empty. fn receive_commands( rx: &mut mpsc::Receiver, + driver: &mut Driver, ) -> Result<(), Box> { const MAX_COMMANDS: u8 = 64; let mut commands_processed = 0; loop { match rx.try_recv() { - Ok(cmd) => { - if let SourceCommand::Stop = cmd { - return Err("Device stopped".into()); + Ok(cmd) => match cmd { + SourceCommand::WriteEvent(_) => (), + SourceCommand::UploadEffect(_, _) => (), + SourceCommand::UpdateEffect(_, _) => (), + SourceCommand::EraseEffect(_, _) => (), + SourceCommand::GetSampleRate(kind, tx) => { + let result = driver.get_sample_rate(kind.as_str()); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send GetSampleRate, {e:?}"); + } } - } + SourceCommand::GetSampleRatesAvail(kind, tx) => { + let result = driver.get_sample_rates_avail(kind.as_str()); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send GetSampleRatesAvail, {e:?}"); + } + } + SourceCommand::SetSampleRate(kind, sample_rate, tx) => { + let result = driver.set_sample_rate(kind.as_str(), sample_rate); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send SetSampleRate, {e:?}"); + } + } + SourceCommand::GetScale(kind, tx) => { + let result = driver.get_scale(kind.as_str()); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send GetScale, {e:?}"); + } + } + SourceCommand::GetScalesAvail(kind, tx) => { + let result = driver.get_scales_avail(kind.as_str()); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send GetScalesAvail, {e:?}"); + } + } + SourceCommand::SetScale(kind, scale, tx) => { + let result = driver.set_scale(kind.as_str(), scale); + let send_result = match result { + Ok(rate) => tx.send(Ok(rate)), + Err(e) => tx.send(Err(e.to_string().into())), + }; + if let Err(e) = send_result { + log::error!("Failed to send SetScale, {e:?}"); + } + } + SourceCommand::Stop => return Err("Device stopped".into()), + }, Err(e) => match e { TryRecvError::Empty => return Ok(()), TryRecvError::Disconnected => { diff --git a/src/input/source/mod.rs b/src/input/source/mod.rs index 3122be7..98e4f53 100644 --- a/src/input/source/mod.rs +++ b/src/input/source/mod.rs @@ -75,5 +75,25 @@ pub enum SourceCommand { ), UpdateEffect(i16, FFEffectData), EraseEffect(i16, Sender>>), + GetSampleRate(String, Sender>>), + GetSampleRatesAvail( + String, + Sender, Box>>, + ), + SetSampleRate( + String, + f64, + Sender>>, + ), + GetScale(String, Sender>>), + GetScalesAvail( + String, + Sender, Box>>, + ), + SetScale( + String, + f64, + Sender>>, + ), Stop, }