diff --git a/README.md b/README.md index 2f79741..9658a97 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,3 @@ -# Status -This repository is in maintenance mode! Garmin has signaled the end of the ANT/ANT+ ecosystem, therefore I will not be adding any new features, feel free to send PRs and I will review and integrate. - -see `development` branch for latest version. - # ant-rs A general purpose ANT* crate for rust diff --git a/ant/Cargo.toml b/ant/Cargo.toml index 7fdbbe6..a7c3287 100644 --- a/ant/Cargo.toml +++ b/ant/Cargo.toml @@ -21,12 +21,9 @@ ant-derive = { path = "../ant-derive", version = "0.1" } embedded-hal = "1.0" embedded-hal-nb = "1.0" nb = "1.1" -rusb = {version = "0.9", optional = true} +rusb = {version = "=0.9.3", optional = true} derive-new = {version = "0.6", default-features = false} -[target.'cfg(target_os = "linux")'.dev-dependencies] -linux-embedded-hal = "0.4" - [dev-dependencies] dialoguer = "0.11" inner = "0.1" @@ -41,4 +38,3 @@ usb_adapter = ["dep:rusb", "usb"] [[test]] name = "serial" - diff --git a/ant/examples/linux_usb_discovery.rs b/ant/examples/linux_usb_discovery.rs new file mode 100644 index 0000000..4c18f9a --- /dev/null +++ b/ant/examples/linux_usb_discovery.rs @@ -0,0 +1,87 @@ +use ant::drivers::*; +use ant::messages::config::{AssignChannel, ChannelId, ChannelPeriod, ChannelRfFrequency, ChannelType, DeviceType, LibConfig, SetNetworkKey, TransmissionType, UnAssignChannel}; +use ant::messages::control::{OpenChannel, OpenRxScanMode, ResetSystem}; +use ant::messages::data::{AcknowledgedData, BroadcastData}; +use rusb::{Device, DeviceList}; + +use dialoguer::Select; + +fn main() { + let mut devices: Vec> = DeviceList::new() + .expect("Unable to lookup usb devices") + .iter() + .filter(|x| is_ant_usb_device_from_device(x)) + .collect(); + + if devices.is_empty() { + panic!("No devices found"); + } + + let device = if devices.len() == 1 { + devices.remove(0) + } else { + let selection = Select::new() + .with_prompt("Multiple devices found, please select a radio to use.") + .items( + &devices + .iter() + .map(|x| x.device_descriptor().unwrap()) + .map(|x| format!("{:04x}:{:04x}", x.vendor_id(), x.product_id())) + .collect::>(), + ) + .interact() + .expect("Dialogue error"); + devices.remove(selection) + }; + + let mut driver = UsbDriver::new(device).unwrap(); + let assign = AssignChannel::new(0, ChannelType::BidirectionalSlave, 0, None); + let key = SetNetworkKey::new(0, [0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45]); + // let key = SetNetworkKey::new(0, [0, 0, 0, 0, 0, 0, 0, 0]); + let rf = ChannelRfFrequency::new(0, 57); + let id = ChannelId::new( + 0, + 0, + // DeviceType::new(17.into(), true), + DeviceType::new_wildcard(), + TransmissionType::new_wildcard(), + ); + let period = ChannelPeriod::new(0, 8192); + let libconfig = LibConfig::new(true, true, true); + driver + .send_message(&ResetSystem::new()) + .expect("Failed to reset device"); + driver.send_message(&key).expect("Message failed"); + driver.send_message(&assign).expect("Message failed"); + driver.send_message(&id).expect("Message failed"); + driver.send_message(&period).expect("Message failed"); + driver.send_message(&rf).expect("Message failed"); + driver.send_message(&libconfig).expect("Message failed"); + driver + .send_message(&OpenRxScanMode::new(Some(false))) + .expect("Message failed"); + + loop { + match driver.get_message() { + Ok(None) => (), + Ok(Some(msg)) => { + match &msg.message { + ant::messages::RxMessage::ChannelEvent(x) => println!("{:#?}", x.payload.message_code), + ant::messages::RxMessage::BroadcastData(x) => { + println!( + "Type: {}, Device number: {}, pairing bit: {}", + x.extended_info.unwrap().channel_id_output.unwrap().device_type.device_type_id, + x.extended_info.unwrap().channel_id_output.unwrap().device_number, + x.extended_info.unwrap().channel_id_output.unwrap().device_type.pairing_request + ); + if x.extended_info.unwrap().channel_id_output.unwrap().device_type.device_type_id != 17.into() { + continue; + } + } + _ => println!("{:#?}", msg), + } + }, + msg => println!("{:#?}", msg), + } + } +} diff --git a/ant/examples/linux_usb_fec_display.rs b/ant/examples/linux_usb_fec_display.rs new file mode 100644 index 0000000..da28f8d --- /dev/null +++ b/ant/examples/linux_usb_fec_display.rs @@ -0,0 +1,145 @@ +use std::time::{Duration, Instant}; + +use ant::channel::{RxError, RxHandler, TxError, TxHandler}; +use ant::drivers::{is_ant_usb_device_from_device, UsbDriver}; +use ant::messages::channel::MessageCode; +use ant::messages::config::SetNetworkKey; +use ant::messages::RxMessage; +use ant::plus::profiles::fitness_equipment_controls::{Display, DisplayConfig, Period}; +use ant::router::Router; +use dialoguer::Select; +use rusb::{Device, DeviceList}; + +use thingbuf::mpsc::errors::{TryRecvError, TrySendError}; +use thingbuf::mpsc::{channel, Receiver, Sender}; + +struct TxSender { + sender: Sender, +} + +struct RxReceiver { + receiver: Receiver, +} + +impl TxHandler for TxSender { + fn try_send(&self, msg: T) -> Result<(), TxError> { + match self.sender.try_send(msg) { + Ok(_) => Ok(()), + Err(TrySendError::Full(_)) => Err(TxError::Full), + Err(TrySendError::Closed(_)) => Err(TxError::Closed), + Err(_) => Err(TxError::UnknownError), + } + } +} + +impl RxHandler for RxReceiver { + fn try_recv(&self) -> Result { + match self.receiver.try_recv() { + Ok(e) => Ok(e), + Err(TryRecvError::Empty) => Err(RxError::Empty), + Err(TryRecvError::Closed) => Err(RxError::Closed), + Err(_) => Err(RxError::UnknownError), + } + } +} + +fn main() -> std::io::Result<()> { + let mut devices: Vec> = DeviceList::new() + .expect("Unable to lookup usb devices") + .iter() + .filter(|x| is_ant_usb_device_from_device(x)) + .collect(); + + if devices.is_empty() { + panic!("No devices found"); + } + + let device = if devices.len() == 1 { + devices.remove(0) + } else { + let selection = Select::new() + .with_prompt("Multiple devices found, please select a radio to use.") + .items( + &devices + .iter() + .map(|x| x.device_descriptor().unwrap()) + .map(|x| format!("{:04x}:{:04x}", x.vendor_id(), x.product_id())) + .collect::>(), + ) + .interact() + .expect("Selection failed"); + devices.remove(selection) + }; + + let driver = UsbDriver::new(device).unwrap(); + + let (channel_tx, router_rx) = channel(8); + let (router_tx, channel_rx) = channel(8); + + let mut router = Router::new( + driver, + RxReceiver { + receiver: router_rx, + }, + ) + .unwrap(); + let snk = SetNetworkKey::new(0, [0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45]); + router.send(&snk).expect("failed to set network key"); + let chan = router + .add_channel(TxSender { sender: router_tx }) + .expect("Add channel failed"); + let config = DisplayConfig { + device_number: 0, + device_number_extension: 0.into(), + channel: chan, + period: Period::FourHz, + ant_plus_key_index: 0, + }; + let mut tacx = Display::new( + config, + TxSender { sender: channel_tx }, + RxReceiver { receiver: channel_rx }, + ); + tacx.set_rx_message_callback(Some(|msg| { + match msg.message { + RxMessage::ChannelEvent(event) => match event.payload.message_code { + MessageCode::EventTransferTxCompleted => println!("Transfer TX completed"), + MessageCode::EventTransferTxFailed => println!("Transfer TX failed"), + _ => {} + }, + _ => {} + } + })); + + tacx.open(); + + let mut last_send_time = Instant::now(); + let mut resistance_multiplier = 2; + let mut has_set_init_values = false; + + loop { + router.process().unwrap(); + tacx.process().unwrap(); + + // Every 5 seconds, increase the target power by 50 watts + if last_send_time.elapsed() >= Duration::from_secs(5) { + if !has_set_init_values { + tacx.set_user_configuration(70, 0, 12, 68, 0).unwrap(); + // tacx.set_basic_resistance(20).unwrap(); + // tacx.set_track_resistance(0, 0.002).unwrap(); + // tacx.set_wind_resistance(0).unwrap(); + has_set_init_values = true; + } + + let target_power = 50 * resistance_multiplier; + println!("Setting power target to {} watts", target_power); + + if let Err(_) = tacx.set_power_target(target_power) { + println!("Failed to send power target"); + } + + resistance_multiplier += 1; + last_send_time = Instant::now(); + } + } +} diff --git a/ant/examples/linux_usb_multi_channel.rs b/ant/examples/linux_usb_multi_channel.rs new file mode 100644 index 0000000..94af232 --- /dev/null +++ b/ant/examples/linux_usb_multi_channel.rs @@ -0,0 +1,235 @@ +use std::cell::OnceCell; +use std::sync::OnceLock; +use std::time::Instant; + +use ant::channel::{RxError, RxHandler, TxError, TxHandler}; +use ant::drivers::{is_ant_usb_device_from_device, UsbDriver}; +use ant::messages::channel::MessageCode; +use ant::messages::config::{LibConfig, SetNetworkKey}; +use ant::messages::data::BroadcastData; +use ant::messages::test_mode::CwTest; +use ant::messages::{AntMessage, RxMessage, TxMessage}; +use ant::plus::profiles::{discovery, fitness_equipment_controls, speed_and_cadence}; +use ant::router::Router; +use dialoguer::Select; +use rusb::{Device, DeviceList}; + +use thingbuf::mpsc::errors::{TryRecvError, TrySendError}; +use thingbuf::mpsc::{channel, Receiver, Sender}; + +struct TxSender { + sender: Sender, +} + +struct RxReceiver { + receiver: Receiver, +} + +impl TxHandler for TxSender { + fn try_send(&self, msg: T) -> Result<(), TxError> { + match self.sender.try_send(msg) { + Ok(_) => Ok(()), + Err(TrySendError::Full(_)) => Err(TxError::Full), + Err(TrySendError::Closed(_)) => Err(TxError::Closed), + Err(_) => Err(TxError::UnknownError), + } + } +} + +impl RxHandler for RxReceiver { + fn try_recv(&self) -> Result { + match self.receiver.try_recv() { + Ok(e) => Ok(e), + Err(TryRecvError::Empty) => Err(RxError::Empty), + Err(TryRecvError::Closed) => Err(RxError::Closed), + Err(_) => Err(RxError::UnknownError), + } + } +} + +fn main() -> std::io::Result<()> { + let mut devices: Vec> = DeviceList::new() + .expect("Unable to lookup usb devices") + .iter() + .filter(|x| is_ant_usb_device_from_device(x)) + .collect(); + + if devices.is_empty() { + panic!("No devices found"); + } + + let device = if devices.len() == 1 { + devices.remove(0) + } else { + let selection = Select::new() + .with_prompt("Multiple devices found, please select a radio to use.") + .items( + &devices + .iter() + .map(|x| x.device_descriptor().unwrap()) + .map(|x| format!("{:04x}:{:04x}", x.vendor_id(), x.product_id())) + .collect::>(), + ) + .interact() + .expect("Selection failed"); + devices.remove(selection) + }; + + let driver = UsbDriver::new(device).unwrap(); + + let (channel_tx, router_rx) = channel(8); + + let mut router = Router::new( + driver, + RxReceiver { + receiver: router_rx, + }, + ) + .unwrap(); + let snk = SetNetworkKey::new(0, [0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45]); + router.send(&snk).expect("failed to set network key"); + + let mut discovery = setup_discovery_channel(&mut router, channel_tx.clone()); + let mut tacx: Option, RxReceiver>> = None; + // let mut tacx2 = setup_sac_channel(&mut router, channel_tx.clone()); + + router.send(&LibConfig::new(true, false, false)).unwrap(); + + let mut last_time = Instant::now(); + let mut started = false; + + loop { + let _ = router.process(); + discovery.process().unwrap(); + if let Some(tacx) = &mut tacx { + tacx.process().unwrap(); + } + // tacx2.process().unwrap(); + + let elapsed = last_time.elapsed(); + if elapsed.as_secs() > 10 && !started { + tacx = Some(setup_fec_channel(&mut router, channel_tx.clone())); + started = true; + } + } +} + +fn setup_fec_channel( + router: &mut Router, + TxSender, RxReceiver>, channel_tx: Sender +) -> fitness_equipment_controls::Display, RxReceiver> { + let (router_tx, channel_rx) = channel(8); + let chan = router + .add_channel(TxSender { sender: router_tx }) + .expect("Add channel failed"); + let tacx_config = fitness_equipment_controls::DisplayConfig { + device_number: 9609, + device_number_extension: 0.into(), + channel: chan, + period: fitness_equipment_controls::Period::FourHz, + ant_plus_key_index: 0, + }; + let mut tacx = fitness_equipment_controls::Display::new( + tacx_config, + TxSender { sender: channel_tx }, + RxReceiver { receiver: channel_rx }, + ); + tacx.set_rx_message_callback(Some(|msg| { + println!("{:#?}", msg); + // match msg.message { + // RxMessage::ChannelEvent(event) => match event.payload.message_code { + // MessageCode::EventTransferTxCompleted => println!("Transfer TX completed"), + // MessageCode::EventTransferTxFailed => println!("Transfer TX failed"), + // _ => {} + // }, + // RxMessage::BroadcastData(x) => + // println!("17: {:x?}", x.payload.channel_number), + // _ => {} + // } + })); + + tacx.open(); + + tacx +} + +fn setup_sac_channel( + router: &mut Router, + TxSender, RxReceiver>, channel_tx: Sender +) -> speed_and_cadence::Display, RxReceiver> { + let (router_tx, channel_rx) = channel(8); + let chan = router + .add_channel(TxSender { sender: router_tx }) + .expect("Add channel failed"); + let tacx_config = speed_and_cadence::DisplayConfig { + device_number: 0, + device_number_extension: 0.into(), + channel: chan, + period: speed_and_cadence::Period::FourHz, + ant_plus_key_index: 0, + }; + let mut tacx = speed_and_cadence::Display::new( + tacx_config, + TxSender { sender: channel_tx }, + RxReceiver { receiver: channel_rx }, + ); + tacx.set_rx_message_callback(Some(|msg| { + match msg.message { + RxMessage::ChannelEvent(event) => match event.payload.message_code { + MessageCode::EventTransferTxCompleted => println!("Transfer TX completed"), + MessageCode::EventTransferTxFailed => println!("Transfer TX failed"), + _ => {} + }, + RxMessage::BroadcastData(x) => + println!("11: {:x?}", x.payload.channel_number), + _ => {} + } + })); + + tacx.open(); + + tacx +} + +fn setup_discovery_channel( + router: &mut Router, + TxSender, RxReceiver>, channel_tx: Sender +) -> discovery::Display, RxReceiver> { + let (router_tx, channel_rx) = channel(8); + let chan = router + .add_channel(TxSender { sender: router_tx }) + .expect("Add channel failed"); + let tacx_config = discovery::DisplayConfig { + channel: chan, + ant_plus_key_index: 0, + }; + let mut tacx = discovery::Display::new( + tacx_config, + TxSender { sender: channel_tx }, + RxReceiver { receiver: channel_rx }, + ); + tacx.set_rx_message_callback(Some(|msg| { + // println!("{:#?}", msg); + match msg.message { + RxMessage::ChannelEvent(event) => match event.payload.message_code { + MessageCode::EventTransferTxCompleted => println!("Transfer TX completed"), + MessageCode::EventTransferTxFailed => println!("Transfer TX failed"), + _ => {} + }, + RxMessage::BroadcastData(x) => { + if x.payload.data[0] == 80 || x.payload.data[0] == 81 { + println!("{:#?}", x); + } else { + println!("discovered"); + } + }, + RxMessage::SerialNumber(x) => + println!("Serial Number: {:#?}", x), + _ => {} + } + })); + + tacx.open(); + + tacx +} \ No newline at end of file diff --git a/ant/examples/linux_usb_router_discovery.rs b/ant/examples/linux_usb_router_discovery.rs new file mode 100644 index 0000000..bbe209a --- /dev/null +++ b/ant/examples/linux_usb_router_discovery.rs @@ -0,0 +1,239 @@ +use std::thread::sleep; +use std::time::Duration; + +use ant::channel::{RxError, RxHandler, TxError, TxHandler}; +use ant::drivers::*; +use ant::messages::config::{AssignChannel, ChannelId, ChannelPeriod, ChannelRfFrequency, ChannelType, DeviceType, LibConfig, SetNetworkKey, TransmissionType, UnAssignChannel}; +use ant::messages::control::{OpenChannel, OpenRxScanMode, ResetSystem}; +use ant::messages::data::{AcknowledgedData, BroadcastData}; +use ant::router::Router; +use rusb::{Device, DeviceList}; + +use dialoguer::Select; + +use thingbuf::mpsc::errors::{TryRecvError, TrySendError}; +use thingbuf::mpsc::{channel, Receiver, Sender}; + +struct TxSender { + sender: Sender, +} + +struct RxReceiver { + receiver: Receiver, +} + +impl TxHandler for TxSender { + fn try_send(&self, msg: T) -> Result<(), TxError> { + match self.sender.try_send(msg) { + Ok(_) => Ok(()), + Err(TrySendError::Full(_)) => Err(TxError::Full), + Err(TrySendError::Closed(_)) => Err(TxError::Closed), + Err(_) => Err(TxError::UnknownError), + } + } +} + +impl RxHandler for RxReceiver { + fn try_recv(&self) -> Result { + match self.receiver.try_recv() { + Ok(e) => Ok(e), + Err(TryRecvError::Empty) => Err(RxError::Empty), + Err(TryRecvError::Closed) => Err(RxError::Closed), + Err(_) => Err(RxError::UnknownError), + } + } +} + +fn main() { + let mut devices: Vec> = DeviceList::new() + .expect("Unable to lookup usb devices") + .iter() + .filter(|x| is_ant_usb_device_from_device(x)) + .collect(); + + if devices.is_empty() { + panic!("No devices found"); + } + + let device = if devices.len() == 1 { + devices.remove(0) + } else { + let selection = Select::new() + .with_prompt("Multiple devices found, please select a radio to use.") + .items( + &devices + .iter() + .map(|x| x.device_descriptor().unwrap()) + .map(|x| format!("{:04x}:{:04x}", x.vendor_id(), x.product_id())) + .collect::>(), + ) + .interact() + .expect("Dialogue error"); + devices.remove(selection) + }; + + let driver = UsbDriver::new(device).unwrap(); + + let (channel_tx, router_rx) = channel(16); + let (router_tx, channel_rx) = channel(16); + + let mut router = Router::new( + driver, + RxReceiver { + receiver: router_rx, + }, + ) + .unwrap(); + + router.set_rx_message_callback(Some(|msg| { + println!("{:?}", msg); + })); + + let snk = SetNetworkKey::new(0, [0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45]); + router.send(&snk).expect("failed to set network key"); + let chan = router + .add_channel(TxSender { sender: router_tx }) + .expect("Add channel failed"); + + let assign = AssignChannel::new(chan, ChannelType::BidirectionalSlave, 0, None); + let rf = ChannelRfFrequency::new(chan, 57); + let id = ChannelId::new( + chan, + 0, + DeviceType::new_wildcard(), + TransmissionType::new_wildcard(), + ); + let period = ChannelPeriod::new(chan, 8192); + let libconfig = LibConfig::new(true, true, true); + channel_tx.try_send(assign.into()).expect("Message failed"); + channel_tx.try_send(id.into()).expect("Message failed"); + channel_tx.try_send(period.into()).expect("Message failed"); + channel_tx.try_send(rf.into()).expect("Message failed"); + channel_tx.try_send(libconfig.into()).expect("Message failed"); + + channel_tx.try_send(OpenRxScanMode::new(Some(false)).into()) + .expect("Message failed"); + + let broadcast_data: BroadcastData; + loop { + router.process(); + match channel_rx.try_recv() { + Ok(msg) => { + match &msg.message { + ant::messages::RxMessage::ChannelEvent(x) => println!("{:#?}", x.payload.message_code), + ant::messages::RxMessage::BroadcastData(x) => { + println!( + "Type: {}, Device number: {}, pairing bit: {}", + x.extended_info.unwrap().channel_id_output.unwrap().device_type.device_type_id, + x.extended_info.unwrap().channel_id_output.unwrap().device_number, + x.extended_info.unwrap().channel_id_output.unwrap().device_type.pairing_request + ); + if x.extended_info.unwrap().channel_id_output.unwrap().device_type.device_type_id != 17.into() { + continue; + } + + // broadcast_data = *x; + // break; + } + _ => println!("{:#?}", msg), + } + }, + Err(TryRecvError::Empty) => {}, + Err(err) => println!("{:?}", err), + // msg => println!("{:#?}", msg), + _ => {}, + } + sleep(Duration::from_millis(100)); + } + + let chan = 0; + driver.send_message(&UnAssignChannel::new(chan)).expect("Unable to unassign channel"); + driver.send_message(&ResetSystem::new()).expect("Unable to reset system"); + + // wait 5 seconds + std::thread::sleep(std::time::Duration::from_secs(5)); + + // driver.send_message(&key).expect("Message failed"); + + let assign = AssignChannel::new(chan, ChannelType::BidirectionalSlave, 0, None); + let id = ChannelId::new( + chan, + broadcast_data.extended_info.unwrap().channel_id_output.unwrap().device_number, + DeviceType::new(17.into(), true), + TransmissionType::new_wildcard() + ); + let rf = ChannelRfFrequency::new(chan, 57); + let period = ChannelPeriod::new(chan, 8192); + + // driver + // .send_message(&ResetSystem::new()) + // .expect("Failed to reset device"); + + // driver.send_message(&key).expect("Message failed"); + driver.send_message(&assign).expect("Message failed"); + println!("Sending id"); + driver.send_message(&id).expect("Message failed"); + driver.send_message(&period).expect("Message failed"); + driver.send_message(&rf).expect("Message failed"); + driver.send_message(&libconfig).expect("Message failed"); + driver.send_message(&OpenChannel::new(0)).expect("Unable to open channel"); + // driver + // .send_message(&OpenRxScanMode::new(Some(false))) + // .expect("Message failed"); + + let mut has_send_target_power = false; + let mut last_send_time = std::time::Instant::now(); + + loop { + match driver.get_message() { + Ok(None) => (), + msg => { + match msg { + Ok(Some(msg)) => { + if let ant::messages::RxMessage::ChannelEvent(x) = &msg.message { + println!("Channel event: {:?}", x); + continue; + } + + // if !has_send_target_power { + // println!("Received message: (hidden)"); + // } else { + // } + // println!("{:#?}", msg.message); + + if !has_send_target_power && last_send_time.elapsed().as_secs() > 5 { + let target_power = 100; + println!("Setting target power to {} : {}", (target_power & 0xFF) as u8, (target_power >> 8) as u8); + let message: ant::messages::TxMessage = AcknowledgedData::new(chan, [ + 0x31, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + 0xFF, + (target_power & 0xFF) as u8, + (target_power >> 8) as u8, + ]).into(); + // let message: ant::messages::TxMessage = AcknowledgedData::new(chan, [ + // 0x46, + // 0xFF, + // 0xFF, + // 0x00, + // 0x00, + // 0x80, + // 0x31, + // 0x01, + // ]).into(); + driver.send_message(&message).expect("Failed to send message"); + println!("Sent target power"); + has_send_target_power = true; + last_send_time = std::time::Instant::now(); + } + }, + Ok(None) => println!("No message received"), + Err(e) => println!("Error receiving message: {:?}", e) + } + }, + } + } +} diff --git a/ant/src/channel.rs b/ant/src/channel.rs index f3b288d..79b5d51 100644 --- a/ant/src/channel.rs +++ b/ant/src/channel.rs @@ -9,6 +9,7 @@ use const_utils::u64::min; use core::time::Duration; +// TODO move this somewhere more appropriate /// Helper to convert durations to search timeouts. /// Anything greater than or equal to 637.5s will default to inifinite timeout per ANT spec. pub const fn duration_to_search_timeout(t: Duration) -> u8 { diff --git a/ant/src/drivers/usb.rs b/ant/src/drivers/usb.rs index c7d7849..a379595 100644 --- a/ant/src/drivers/usb.rs +++ b/ant/src/drivers/usb.rs @@ -123,7 +123,7 @@ fn find_endpoint( impl UsbDriver { pub fn new(device: Device) -> Result { - let handle = match device.open() { + let mut handle = match device.open() { Ok(h) => h, Err(e) => return Err(UsbError::FailedToOpenDevice(e)), }; @@ -177,7 +177,7 @@ impl UsbDriver { }) } - pub fn release(self) -> Result, rusb::Error> { + pub fn release(mut self) -> Result, rusb::Error> { // reatach all drivers and undo usb walk // TODO cast into local error type self.handle.release_interface(self.iface)?; diff --git a/ant/src/messages/control.rs b/ant/src/messages/control.rs index c2b7af6..5fe61d4 100644 --- a/ant/src/messages/control.rs +++ b/ant/src/messages/control.rs @@ -116,6 +116,12 @@ impl TransmitableMessage for OpenRxScanMode { } } +impl From for TxMessage { + fn from(msg: OpenRxScanMode) -> TxMessage { + TxMessage::OpenRxScanMode(msg) + } +} + #[derive(PackedStruct, AntTx, new, Clone, Copy, Debug, Default, PartialEq)] #[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "1")] pub struct SleepMessage { diff --git a/ant/src/messages/mod.rs b/ant/src/messages/mod.rs index 5fe26e6..811b810 100644 --- a/ant/src/messages/mod.rs +++ b/ant/src/messages/mod.rs @@ -6,7 +6,7 @@ // option. This file may not be copied, modified, or distributed // except according to those terms. -use crate::messages::config::{ +use crate::{messages::{config::{ AddChannelIdToList, AddEncryptionIdToList, AssignChannel, ChannelId, ChannelPeriod, ChannelRfFrequency, ChannelSearchPriority, ChannelSearchSharing, ConfigEncryptionIdList, ConfigIdList, ConfigureAdvancedBurst, ConfigureEventBuffer, ConfigureEventFilter, @@ -17,7 +17,7 @@ use crate::messages::config::{ SetEncryptionInfoEncryptionId, SetEncryptionInfoRandomSeed, SetEncryptionInfoUserInformationString, SetEncryptionKey, SetNetworkKey, SetSelectiveDataUpdateMask, StoreEncryptionKeyInNvm, TransmitPower, UnAssignChannel, -}; +}, control::OpenRxScanMode}}; use channel::{ChannelEvent, ChannelResponse}; use control::{CloseChannel, OpenChannel, RequestMessage, ResetSystem, SleepMessage}; use data::{ @@ -124,7 +124,7 @@ pub enum TxMessage { OpenChannel(OpenChannel), CloseChannel(CloseChannel), RequestMessage(RequestMessage), - // OpenRxScanMode(OpenRxScanMode), + OpenRxScanMode(OpenRxScanMode), SleepMessage(SleepMessage), BroadcastData(BroadcastData), AcknowledgedData(AcknowledgedData), @@ -188,7 +188,7 @@ impl TransmitableMessage for TxMessage { TxMessage::OpenChannel(oc) => oc.serialize_message(buf), TxMessage::CloseChannel(cc) => cc.serialize_message(buf), TxMessage::RequestMessage(rm) => rm.serialize_message(buf), - // TxMessage::OpenRxScanMode(or) => or.serialize_message(buf), + TxMessage::OpenRxScanMode(or) => or.serialize_message(buf), TxMessage::SleepMessage(sm) => sm.serialize_message(buf), TxMessage::BroadcastData(bd) => bd.serialize_message(buf), TxMessage::AcknowledgedData(ad) => ad.serialize_message(buf), @@ -245,7 +245,7 @@ impl TransmitableMessage for TxMessage { TxMessage::OpenChannel(oc) => oc.get_tx_msg_id(), TxMessage::CloseChannel(cc) => cc.get_tx_msg_id(), TxMessage::RequestMessage(rm) => rm.get_tx_msg_id(), - // TODO TxMessage::OpenRxScanMode(or) => or.serialize_message(buf), + TxMessage::OpenRxScanMode(or) => or.get_tx_msg_id(), TxMessage::SleepMessage(sm) => sm.get_tx_msg_id(), TxMessage::BroadcastData(bd) => bd.get_tx_msg_id(), TxMessage::AcknowledgedData(ad) => ad.get_tx_msg_id(), diff --git a/ant/src/messages/requested_response.rs b/ant/src/messages/requested_response.rs index 5d5373e..0827e54 100644 --- a/ant/src/messages/requested_response.rs +++ b/ant/src/messages/requested_response.rs @@ -328,7 +328,7 @@ pub struct AdvancedBurstCapabilities { pub supported_features: SupportedFeatures, } -#[derive(PackedStruct, Debug, Clone, PartialEq)] +#[derive(PackedStruct, Debug, Clone, Copy, PartialEq)] #[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "4")] pub struct SerialNumber { #[packed_field(bytes = "0:3")] diff --git a/ant/src/plus/common/msg_handler.rs b/ant/src/plus/common/msg_handler.rs index f4dd66c..fc9abea 100644 --- a/ant/src/plus/common/msg_handler.rs +++ b/ant/src/plus/common/msg_handler.rs @@ -707,7 +707,24 @@ mod tests { #[test] fn state_transition_on_failure() { - // TODO + let mut msg_handler = MessageHandler::new(&get_config()); + assert!(msg_handler.configure_state.get_state() != ConfigureStateId::Error); + get_config_message(&mut msg_handler, TxMessageId::AssignChannel); + let result = msg_handler.receive_message( &AntMessage { + header: RxMessageHeader { + sync: RxSyncByte::Write, + msg_id: crate::messages::RxMessageId::ChannelEvent, + msg_length: 3, + }, + message: RxMessage::ChannelResponse(ChannelResponse { + channel_number: 0, + message_id: TxMessageId::AssignChannel, + message_code: MessageCode::InvalidMessage, + }), + checksum: 123, // this doesn't matter + }); + assert!(msg_handler.configure_state.get_state() == ConfigureStateId::Error); + assert!(result.is_err()); } #[test] @@ -732,6 +749,42 @@ mod tests { #[test] fn reset() { - // TODO + let mut msg_handler = MessageHandler::new(&get_config()); + get_config_message(&mut msg_handler, TxMessageId::AssignChannel); + let _ = msg_handler.receive_message( &AntMessage { + header: RxMessageHeader { + sync: RxSyncByte::Write, + msg_id: crate::messages::RxMessageId::ChannelEvent, + msg_length: 3, + }, + message: RxMessage::ChannelResponse(ChannelResponse { + channel_number: 0, + message_id: TxMessageId::AssignChannel, + message_code: MessageCode::InvalidMessage, + }), + checksum: 123, // this doesn't matter + }); + msg_handler.state_config.device_number = 25; + assert!(msg_handler.configure_state.get_state() == ConfigureStateId::Error); + msg_handler.reset_state(false); + assert!(msg_handler.configure_state.get_state() == ConfigureStateId::UnknownClose); + assert!(msg_handler.state_config.device_number == 25); + msg_handler.reset_state(true); + assert!(msg_handler.state_config.device_number == get_config().device_number); + } + + #[test] + fn tx_ready_after_open() { + let mut msg_handler = MessageHandler::new(&get_config()); + assert!(!msg_handler.is_tx_ready()); + get_config_message(&mut msg_handler, TxMessageId::SearchTimeout); + // end of config + assert!(!msg_handler.is_tx_ready()); + msg_handler + .receive_message(&get_response_ok(TxMessageId::SearchTimeout)) + .expect("State machine error"); + msg_handler.open(); + get_config_message(&mut msg_handler, TxMessageId::OpenChannel); + assert!(msg_handler.is_tx_ready()); } } diff --git a/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs b/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs new file mode 100644 index 0000000..51f68bf --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs @@ -0,0 +1,129 @@ +pub use crate::plus::common::datapages::BatteryStatusField; +use ant_derive::DataPage; +use derive_new::new; +use packed_struct::prelude::*; + +pub const DATA_PAGE_NUMBER_MASK: u8 = 0x7F; + +#[derive(PrimitiveEnum_u8, PartialEq, Copy, Clone, Debug)] +pub enum DataPageNumbers { + MainDataPage = 16, + PowerDataPage = 25, +} + +impl From for Integer> { + fn from(dp: DataPageNumbers) -> Self { + dp.to_primitive().into() + } +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct MainDataPage { + #[packed_field(bits = "1:7")] + data_page_number: Integer>, + #[packed_field(bits = "0")] + pub page_change_toggle: bool, + #[packed_field(bytes = "1")] + pub equiment_type: u8, + #[packed_field(bytes = "2")] + pub elapsed_time: u8, + #[packed_field(bytes = "3")] + pub distance: u8, + #[packed_field(bytes = "4:5")] + pub speed: u16, + #[packed_field(bytes = "6")] + pub heart_rate: u8, + #[packed_field(bytes = "7")] + pub cap_state_bf: u8, +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct PowerDataPage { + #[packed_field(bit = "0")] + pub page_change_toggle: bool, + #[packed_field(bits = "1:7")] + data_page_number: Integer>, + #[packed_field(byte = "1")] + pub event_count: u8, + #[packed_field(byte = "2")] + pub cadance: u8, + #[packed_field(bytes = "3:4")] + pub accumulated_power: u16, + #[packed_field(bits = "40:51")] + pub instantaneous_power: Integer>, + #[packed_field(bits = "52:55")] + pub trainer_status: u8, + #[packed_field(byte = "7")] + pub flag_state_bf: u8, +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct UserConfigurationDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:2")] + pub user_weight: u16, + #[packed_field(byte = "3")] + _reserved: u8, + #[packed_field(bits = "32:36")] + pub bicycle_wheel: u8, + #[packed_field(bits = "37:47")] + pub bicycle_weight: u16, + #[packed_field(byte = "6")] + pub bicycle_wheel_diameter: u8, + #[packed_field(byte = "7")] + pub gear_ratio: u8, +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct WindResistanceDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:4")] + _reserved: [u8; 4], + #[packed_field(byte = "5")] + pub wind_resistance_coefficient: u8, + #[packed_field(byte = "6")] + pub wind_speed: u8, + #[packed_field(byte = "7")] + pub drafting_factor: u8, +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct TrackResistanceDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:4")] + _reserved: [u8; 4], + #[packed_field(bytes = "5:6")] + pub grade: u16, + #[packed_field(byte = "7")] + pub coefficient_of_rolling_resistance: u8, +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct BasicResistanceDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:6")] + _reserved: [u8; 6], + #[packed_field(byte = "7")] + pub total_resistance: u8, +} + +#[derive(PackedStruct, DataPage, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct TargetPowerDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:5")] + _reserved: [u8; 5], + #[packed_field(bytes = "6:7")] + pub total_power: u16, +} diff --git a/ant/src/plus/profiles/fitness_equipment_controls/display.rs b/ant/src/plus/profiles/fitness_equipment_controls/display.rs new file mode 100644 index 0000000..dde1e4d --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -0,0 +1,370 @@ +use crate::channel::{duration_to_search_timeout, TxError}; +use crate::channel::{ChanError, RxHandler, TxHandler}; +use crate::messages::config::{ + ChannelType, TransmissionChannelType, TransmissionGlobalDataPages, TransmissionType +}; +use crate::messages::data::AcknowledgedData; +use crate::messages::{AntMessage, RxMessage, TxMessage, TxMessageChannelConfig, TxMessageData}; +// use crate::plus::common::datapages::MANUFACTURER_SPECIFIC_RANGE; +use crate::plus::common::msg_handler::{ChannelConfig, MessageHandler}; +use crate::plus::profiles::fitness_equipment_controls::{ + BasicResistanceDataPage, DATA_PAGE_NUMBER_MASK, DEVICE_TYPE, DataPageNumbers, EquipmentType, Error, MainDataPage, MonitorTxDataPage, Period, PowerDataPage, TargetPowerDataPage, TrackResistanceDataPage, UserConfigurationDataPage, WindResistanceDataPage +}; +use crate::plus::NETWORK_RF_FREQUENCY; +use crate::plus::profiles::speed_and_cadence; + +use packed_struct::prelude::{packed_bits::Bits, Integer}; +use packed_struct::{PackedStruct, PrimitiveEnum}; + +use std::time::Duration; + +pub struct Display, R: RxHandler> { + msg_handler: MessageHandler, + rx_message_callback: Option, + rx_datapage_callback: Option)>, + tx_message_callback: Option Option>, + tx_datapage_callback: Option Option>, + tx: T, + rx: R, + equipment_type: Option, + virtual_speed: Option, + real_speed: Option, + elapsed_time: u16, + distance: u16, + cadence: u8, + power: u16, + last_power: u16, + power_event_count: u8, +} + +pub struct DisplayConfig { + pub channel: u8, + pub device_number: u16, + pub device_number_extension: Integer>, + pub ant_plus_key_index: u8, + pub period: Period, +} + +impl, R: RxHandler> Display { + pub fn new( + conf: DisplayConfig, + tx: T, + rx: R, + ) -> Self { + let transmission_type = if conf.device_number_extension == 0.into() { + TransmissionType::new_wildcard() + } else { + TransmissionType::new( + TransmissionChannelType::IndependentChannel, + TransmissionGlobalDataPages::GlobalDataPagesNotUsed, + conf.device_number_extension, + ) + }; + let channel_config = ChannelConfig { + channel: conf.channel, + device_number: conf.device_number, + device_type: DEVICE_TYPE, + channel_type: ChannelType::BidirectionalSlave, + network_key_index: conf.ant_plus_key_index, + transmission_type, + radio_frequency: NETWORK_RF_FREQUENCY, + timeout_duration: duration_to_search_timeout(Duration::from_secs(30)), + channel_period: conf.period.into(), + }; + Self { + rx_message_callback: None, + rx_datapage_callback: None, + tx_message_callback: None, + tx_datapage_callback: None, + msg_handler: MessageHandler::new(&channel_config), + tx, + rx, + equipment_type: None, + virtual_speed: None, + real_speed: None, + elapsed_time: 0, + distance: 0, + cadence: 0, + power: 0, + last_power: 0, + power_event_count: 0, + } + } + + pub fn open(&mut self) { + self.msg_handler.open(); + } + + pub fn close(&mut self) { + self.msg_handler.close(); + } + + pub fn get_device_id(&self) -> u16 { + self.msg_handler.get_device_id() + } + + pub fn set_rx_message_callback(&mut self, f: Option) { + self.rx_message_callback = f; + } + + pub fn set_rx_datapage_callback(&mut self, f: Option)>) { + self.rx_datapage_callback = f; + } + + pub fn set_tx_message_callback(&mut self, f: Option Option>) { + self.tx_message_callback = f; + } + + pub fn set_tx_datapage_callback(&mut self, f: Option Option>) { + self.tx_datapage_callback = f; + } + + pub fn reset_state(&mut self) { + // TODO + } + + // get result and call callback + fn handle_dp(&mut self, data: &[u8; 8]) { + let dp = self.parse_dp(data); + if let Some(f) = self.rx_datapage_callback { + f(dp); + } + } + + fn parse_dp(&mut self, data: &[u8; 8]) -> Result { + let dp_num = data[0] & DATA_PAGE_NUMBER_MASK; + if let Some(dp) = DataPageNumbers::from_primitive(dp_num) { + let parsed = match dp { + DataPageNumbers::MainDataPage => { + let page = MainDataPage::unpack(data)?; + + // Equipment Type + self.equipment_type = Some(page.equiment_type.into()); + + // Elapsed Time + let old_elapsed_time = self.elapsed_time as u16 % 64; + let mut elapsed_time = page.elapsed_time as u16 / 4; + if elapsed_time != self.elapsed_time && old_elapsed_time > elapsed_time { + elapsed_time += 64; + } + self.elapsed_time += elapsed_time - old_elapsed_time; + + // Distance + if page.cap_state_bf & 0x04 > 0 { + let old_distance = self.distance as u16 % 256; + let mut distance = page.distance as u16; + if distance != self.distance && old_distance > distance { + distance += 256; + } + self.distance += distance - old_distance; + } + + // Speed + if page.cap_state_bf & 0x08 > 0 { + self.virtual_speed = Some((page.speed / 1000) as u8); + self.real_speed = None; + } else { + self.real_speed = Some((page.speed / 1000) as u8); + self.virtual_speed = None; + } + + MonitorTxDataPage::MainDataPage(page) + }, + DataPageNumbers::PowerDataPage => { + let page = PowerDataPage::unpack(data)?; + + self.cadence = page.cadance; + + let diff_power = if page.accumulated_power >= self.last_power { + page.accumulated_power - self.last_power + } else { + (65536u32 + page.accumulated_power as u32 - self.last_power as u32) as u16 + }; + + let diff_event_count = if page.event_count >= self.power_event_count { + page.event_count - self.power_event_count + } else { + (256u16 + page.event_count as u16 - self.power_event_count as u16) as u8 + }; + + if diff_event_count > 0 { + self.power = diff_power / diff_event_count as u16; + } else { + // Optionally handle zero difference (e.g., keep previous power or log) + // For now, do nothing to avoid division by zero + } + + self.last_power = page.accumulated_power; + self.power_event_count = page.event_count; + + MonitorTxDataPage::PowerDataPage(page) + }, + }; + return Ok(parsed); + } + // if MANUFACTURER_SPECIFIC_RANGE.contains(&dp_num) { + // return Ok(MonitorTxDataPage::ManufacturerSpecific( + // ManufacturerSpecific::unpack(data)?, + // )); + // } + Err(Error::UnsupportedDataPage(dp_num)) + } + + pub fn process(&mut self) -> Result<(), ChanError> { + // TODO handle closed channel + while let Ok(msg) = self.rx.try_recv() { + if let Some(f) = self.rx_message_callback { + f(&msg); + } + match msg.message { + RxMessage::BroadcastData(msg) => self.handle_dp(&msg.payload.data), + RxMessage::AcknowledgedData(msg) => { + println!("Received acknowledged data: {:?}", msg); + self.handle_dp(&msg.payload.data) + }, + _ => (), + } + match self.msg_handler.receive_message(&msg) { + Ok(_) => (), + Err(e) => { + if let Some(f) = self.rx_datapage_callback { + f(Err(e.into())); + } + } + } + } + + // TODO handle errors + if let Some(msg) = self.msg_handler.send_message() { + println!("Sending message: {:?}", msg); + self.tx.try_send(msg)?; + } + if let Some(callback) = self.tx_message_callback { + if let Some(mut msg) = callback() { + msg.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(msg.into())?; + } + } + if self.msg_handler.is_tx_ready() { + if let Some(callback) = self.tx_datapage_callback { + if let Some(mut msg) = callback() { + println!("Sending data page in process()"); + msg.set_channel(self.msg_handler.get_channel()); + self.msg_handler.tx_sent(); + self.tx.try_send(msg.into())?; + } + } + } + Ok(()) + } + + pub fn get_equipment_type(&self) -> Option { + self.equipment_type + } + + pub fn get_virtual_speed(&self) -> Option { + self.virtual_speed + } + + pub fn get_real_speed(&self) -> Option { + self.real_speed + } + + pub fn get_elapsed_time(&self) -> u16 { + self.elapsed_time + } + + pub fn get_distance(&self) -> u16 { + self.distance + } + + pub fn get_candence(&self) -> u8 { + self.cadence + } + + pub fn get_power(&self) -> u16 { + self.power + } + + pub fn set_user_configuration( + &mut self, + user_weight: u16, + bicycle_wheel: u8, + bicycle_weight: u16, + bicycle_wheel_diameter: u8, + gear_ratio: u8, + ) -> Result<(), TxError> { + let mut message: TxMessageData = AcknowledgedData::new(0, UserConfigurationDataPage::new( + 0x37, + user_weight * 100, + 0, + bicycle_wheel, + bicycle_weight * 20, + bicycle_wheel_diameter, + gear_ratio, + ).pack().unwrap()).into(); + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; + Ok(()) + } + + pub fn set_basic_resistance( + &mut self, + resistance: u8, + ) -> Result<(), TxError> { + let mut message: TxMessageData = AcknowledgedData::new(0, BasicResistanceDataPage::new( + 0x30, + [0, 0, 0, 0, 0, 0], + resistance * 2, + ).pack().unwrap()).into(); + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; + Ok(()) + } + + pub fn set_wind_resistance( + &mut self, + wind_resistance_coefficient: u8, + wind_speed: i8, + drafting_factor: u8, + ) -> Result<(), TxError> { + let mut message: TxMessageData = AcknowledgedData::new(0, WindResistanceDataPage::new( + 0x32, + [0, 0, 0, 0], + wind_resistance_coefficient, + (wind_speed + 127) as u8, + drafting_factor, + ).pack().unwrap()).into(); + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; + Ok(()) + } + + pub fn set_track_resistance( + &mut self, + grade: i16, + coefficient_of_rolling_resistance: f32, + ) -> Result<(), TxError> { + let mut message: TxMessageData = AcknowledgedData::new(0, TrackResistanceDataPage::new( + 0x32, + [0, 0, 0, 0], + (grade + 200 * 100) as u16, + (coefficient_of_rolling_resistance * 20000.0) as u8, + ).pack().unwrap()).into(); + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; + Ok(()) + } + + pub fn set_power_target(&mut self, power: u16) -> Result<(), TxError> { + let power: u16 = power * 4; + let mut message: TxMessageData = AcknowledgedData::new(0, TargetPowerDataPage::new( + 0x31, + [0, 0, 0, 0, 0], + power, + ).pack().unwrap()).into(); + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; + Ok(()) + } +} diff --git a/ant/src/plus/profiles/fitness_equipment_controls/mod.rs b/ant/src/plus/profiles/fitness_equipment_controls/mod.rs new file mode 100644 index 0000000..68a2b3b --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/mod.rs @@ -0,0 +1,94 @@ +//! Based off V5 of the Fitness Equipment specification + +mod datapages; +mod display; + +pub use datapages::*; +pub use display::*; + +// use crate::plus::common::datapages::{ModeSettings, RequestDataPage}; +use crate::plus::common::msg_handler::StateError; + +const DEVICE_TYPE: u8 = 17; + +#[derive(Debug, Default)] +pub enum Period { + #[default] + FourHz, + TwoHz, + OneHz, +} + +impl From for u16 { + fn from(p: Period) -> u16 { + match p { + Period::FourHz => 8192, + Period::TwoHz => 16140, + Period::OneHz => 32280, + } + } +} + +#[derive(PartialEq, Copy, Clone, Debug)] +pub enum MonitorTxDataPage { + MainDataPage(MainDataPage), + PowerDataPage(PowerDataPage), +} + +#[derive(PartialEq, Copy, Clone, Debug)] +pub enum DisplayTxDataPage { + // ManufacturerSpecific(ManufacturerSpecific), + UserConfigurationDataPage(UserConfigurationDataPage), + BasicResistanceDataPage(BasicResistanceDataPage), + WindResistanceDataPage(WindResistanceDataPage), + TrackResistanceDataPage(TrackResistanceDataPage), + TargetPowerDataPage(TargetPowerDataPage), +} + +#[derive(Debug, Clone)] +pub enum Error { + BytePatternError(packed_struct::PackingError), + UnsupportedDataPage(u8), + PageAlreadyPending(), + NotAssociated(), + ConfigurationError(StateError), +} + +impl From for Error { + fn from(err: packed_struct::PackingError) -> Self { + Self::BytePatternError(err) + } +} + +impl From for Error { + fn from(err: StateError) -> Self { + Self::ConfigurationError(err) + } +} + +#[derive(Debug, Clone, Copy)] +pub enum EquipmentType { + Treadmill, + Elliptical, + Reserved, + Rower, + Climber, + NordicSkier, + StationaryBike, + General, +} + +impl From for EquipmentType { + fn from(p: u8) -> EquipmentType { + match p { + 19 => EquipmentType::Treadmill, + 20 => EquipmentType::Elliptical, + 21 => EquipmentType::Reserved, + 22 => EquipmentType::Rower, + 23 => EquipmentType::Climber, + 24 => EquipmentType::NordicSkier, + 25 => EquipmentType::StationaryBike, + _ => EquipmentType::General, + } + } +} diff --git a/ant/src/plus/profiles/heart_rate/.mod.rs.swp b/ant/src/plus/profiles/heart_rate/.mod.rs.swp deleted file mode 100644 index 1b4d2c5..0000000 Binary files a/ant/src/plus/profiles/heart_rate/.mod.rs.swp and /dev/null differ diff --git a/ant/src/plus/profiles/mod.rs b/ant/src/plus/profiles/mod.rs index 8079913..167310b 100644 --- a/ant/src/plus/profiles/mod.rs +++ b/ant/src/plus/profiles/mod.rs @@ -7,3 +7,6 @@ // except according to those terms. pub mod heart_rate; +pub mod speed_and_cadence; +pub mod fitness_equipment_controls; +pub mod discovery; \ No newline at end of file diff --git a/ant/src/plus/profiles/speed_and_cadence/datapages.rs b/ant/src/plus/profiles/speed_and_cadence/datapages.rs new file mode 100644 index 0000000..9ca337e --- /dev/null +++ b/ant/src/plus/profiles/speed_and_cadence/datapages.rs @@ -0,0 +1,24 @@ +pub use crate::plus::common::datapages::BatteryStatusField; +use derive_new::new; +use packed_struct::prelude::*; + +pub const DATA_PAGE_NUMBER_MASK: u8 = 0x7F; + +#[derive(PrimitiveEnum_u8, PartialEq, Copy, Clone, Debug)] +pub enum DataPageNumbers { + MainDataPage = 16, +} + +impl From for Integer> { + fn from(dp: DataPageNumbers) -> Self { + dp.to_primitive().into() + } +} + +// TODO: Implement MainDataPage +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "8")] +pub struct MainDataPage { + #[packed_field(bytes = "0:7")] + _reserved: [u8; 8], +} \ No newline at end of file diff --git a/ant/src/plus/profiles/speed_and_cadence/display.rs b/ant/src/plus/profiles/speed_and_cadence/display.rs new file mode 100644 index 0000000..65ea133 --- /dev/null +++ b/ant/src/plus/profiles/speed_and_cadence/display.rs @@ -0,0 +1,190 @@ +use crate::channel::{duration_to_search_timeout, TxError}; +use crate::channel::{ChanError, RxHandler, TxHandler}; +use crate::messages::config::{ + ChannelType, TransmissionChannelType, TransmissionGlobalDataPages, TransmissionType +}; +use crate::messages::{AntMessage, RxMessage, TxMessage, TxMessageChannelConfig, TxMessageData}; +// use crate::plus::common::datapages::MANUFACTURER_SPECIFIC_RANGE; +use crate::plus::common::msg_handler::{ChannelConfig, MessageHandler}; +use crate::plus::profiles::speed_and_cadence::{ + DataPageNumbers, Error, MainDataPage, MonitorTxDataPage, + Period, DATA_PAGE_NUMBER_MASK, DEVICE_TYPE +}; +use crate::plus::NETWORK_RF_FREQUENCY; + +use packed_struct::prelude::{packed_bits::Bits, Integer}; +use packed_struct::{PackedStruct, PrimitiveEnum}; + +use std::time::Duration; + +pub struct Display, R: RxHandler> { + msg_handler: MessageHandler, + rx_message_callback: Option, + rx_datapage_callback: Option)>, + tx_message_callback: Option Option>, + tx_datapage_callback: Option Option>, + tx: T, + rx: R, +} + +pub struct DisplayConfig { + pub channel: u8, + pub device_number: u16, + pub device_number_extension: Integer>, + pub ant_plus_key_index: u8, + pub period: Period, +} + +impl, R: RxHandler> Display { + pub fn new( + conf: DisplayConfig, + tx: T, + rx: R, + ) -> Self { + let transmission_type = if conf.device_number_extension == 0.into() { + TransmissionType::new_wildcard() + } else { + TransmissionType::new( + TransmissionChannelType::IndependentChannel, + TransmissionGlobalDataPages::GlobalDataPagesNotUsed, + conf.device_number_extension, + ) + }; + let channel_config = ChannelConfig { + channel: conf.channel, + device_number: conf.device_number, + device_type: DEVICE_TYPE, + channel_type: ChannelType::BidirectionalSlave, + network_key_index: conf.ant_plus_key_index, + transmission_type, + radio_frequency: NETWORK_RF_FREQUENCY, + timeout_duration: duration_to_search_timeout(Duration::from_secs(30)), + channel_period: conf.period.into(), + }; + Self { + rx_message_callback: None, + rx_datapage_callback: None, + tx_message_callback: None, + tx_datapage_callback: None, + msg_handler: MessageHandler::new(&channel_config), + tx, + rx, + } + } + + pub fn open(&mut self) { + self.msg_handler.open(); + } + + pub fn close(&mut self) { + self.msg_handler.close(); + } + + pub fn get_device_id(&self) -> u16 { + self.msg_handler.get_device_id() + } + + pub fn set_rx_message_callback(&mut self, f: Option) { + self.rx_message_callback = f; + } + + pub fn set_rx_datapage_callback(&mut self, f: Option)>) { + self.rx_datapage_callback = f; + } + + pub fn set_tx_message_callback(&mut self, f: Option Option>) { + self.tx_message_callback = f; + } + + pub fn set_tx_datapage_callback(&mut self, f: Option Option>) { + self.tx_datapage_callback = f; + } + + pub fn reset_state(&mut self) { + // TODO + } + + // get result and call callback + fn handle_dp(&mut self, data: &[u8; 8]) { + let dp = self.parse_dp(data); + if let Some(f) = self.rx_datapage_callback { + f(dp); + } + } + + fn parse_dp(&mut self, data: &[u8; 8]) -> Result { + let dp_num = data[0] & DATA_PAGE_NUMBER_MASK; + if let Some(dp) = DataPageNumbers::from_primitive(dp_num) { + let parsed = match dp { + DataPageNumbers::MainDataPage => { + let page = MainDataPage::unpack(data)?; + + // TODO: set specific data + + MonitorTxDataPage::MainDataPage(page) + }, + }; + return Ok(parsed); + } + // if MANUFACTURER_SPECIFIC_RANGE.contains(&dp_num) { + // return Ok(MonitorTxDataPage::ManufacturerSpecific( + // ManufacturerSpecific::unpack(data)?, + // )); + // } + Err(Error::UnsupportedDataPage(dp_num)) + } + + pub fn process(&mut self) -> Result<(), ChanError> { + // TODO handle closed channel + while let Ok(msg) = self.rx.try_recv() { + if let Some(f) = self.rx_message_callback { + f(&msg); + } + match msg.message { + RxMessage::BroadcastData(msg) => self.handle_dp(&msg.payload.data), + RxMessage::AcknowledgedData(msg) => { + println!("Received acknowledged data: {:?}", msg); + self.handle_dp(&msg.payload.data) + }, + _ => (), + } + match self.msg_handler.receive_message(&msg) { + Ok(_) => (), + Err(e) => { + if let Some(f) = self.rx_datapage_callback { + f(Err(e.into())); + } + } + } + } + + // TODO handle errors + if let Some(msg) = self.msg_handler.send_message() { + println!("Sending message: {:?}", msg); + self.tx.try_send(msg)?; + } + if let Some(callback) = self.tx_message_callback { + if let Some(mut msg) = callback() { + msg.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(msg.into())?; + } + } + if self.msg_handler.is_tx_ready() { + if let Some(callback) = self.tx_datapage_callback { + if let Some(mut msg) = callback() { + println!("Sending data page in process()"); + msg.set_channel(self.msg_handler.get_channel()); + self.msg_handler.tx_sent(); + self.tx.try_send(msg.into())?; + } + } + } + Ok(()) + } + + pub fn direct_send(&self, message: TxMessage) -> Result<(), TxError> { + println!("Sending message: {:?}", message); + self.tx.try_send(message)?; + Ok(()) + } +} \ No newline at end of file diff --git a/ant/src/plus/profiles/speed_and_cadence/mod.rs b/ant/src/plus/profiles/speed_and_cadence/mod.rs new file mode 100644 index 0000000..5ed7703 --- /dev/null +++ b/ant/src/plus/profiles/speed_and_cadence/mod.rs @@ -0,0 +1,176 @@ +mod datapages; +mod display; + +pub use datapages::*; +pub use display::*; + +use derive_new::new; +use packed_struct::derive::PackedStruct; + +use crate::plus::common::msg_handler::StateError; + +pub const DEVICE_TYPE: u8 = 121; + +#[derive(Debug, Default)] +pub enum Period { + #[default] + FourHz, + TwoHz, + OneHz, +} + +impl From for u16 { + fn from(p: Period) -> u16 { + match p { + Period::FourHz => 8192, + Period::TwoHz => 16140, + Period::OneHz => 32280, + } + } +} + +#[derive(PartialEq, Copy, Clone, Debug)] +pub enum MonitorTxDataPage { + MainDataPage(MainDataPage), +} + +#[derive(PartialEq, Copy, Clone, Debug)] +pub enum DisplayTxDataPage { + // ManufacturerSpecific(ManufacturerSpecific), +} + +#[derive(Debug, Clone)] +pub enum Error { + BytePatternError(packed_struct::PackingError), + UnsupportedDataPage(u8), + PageAlreadyPending(), + NotAssociated(), + ConfigurationError(StateError), +} + +impl From for Error { + fn from(err: packed_struct::PackingError) -> Self { + Self::BytePatternError(err) + } +} + +impl From for Error { + fn from(err: StateError) -> Self { + Self::ConfigurationError(err) + } +} + +#[derive(PackedStruct, new, PartialEq, Copy, Clone, Debug)] +#[packed_struct(endian = "lsb")] +pub struct SpeedAndCadence { + /// Time of the last valid bike cadence event (1/1024 sec) + pub cadence_event_time: u16, + + /// Total number of pedal revolutions + pub cadence_revolution_count: u16, + + /// Time of the last valid bike speed event (1/1024 sec) + pub speed_event_time: u16, + + /// Total number of wheel revolutions + pub speed_revolution_count: u16, +} + +impl SpeedAndCadence { + /// Calculates the average cadence (rpm) + pub fn cadence(a: SpeedAndCadence, b: SpeedAndCadence) -> Option { + let time_delta = b.cadence_event_time.wrapping_sub(a.cadence_event_time); + if time_delta == 0 { + return None; + } + let rev_delta = b + .cadence_revolution_count + .wrapping_sub(a.cadence_revolution_count); + Some((rev_delta as f32) * 1024.0 * 60.0 / (time_delta as f32)) + } + + /// Calculates the number of wheel revolutions + pub fn wheel_revolutions(a: SpeedAndCadence, b: SpeedAndCadence) -> Option { + let time_delta = b.speed_event_time.wrapping_sub(a.speed_event_time); + if time_delta == 0 { + return None; + } + Some( + b.speed_revolution_count + .wrapping_sub(a.speed_revolution_count), + ) + } + + /// Calculates the distance (m) covered between two messages + pub fn distance(a: SpeedAndCadence, b: SpeedAndCadence, circumference: f32) -> Option { + if let Some(revs) = Self::wheel_revolutions(a, b) { + return Some(revs as f32 * circumference); + } + None + } + + /// Calculates average speed in revolutions per sec (useful when circumference is not known) + pub fn speed_revs_per_sec(a: SpeedAndCadence, b: SpeedAndCadence) -> Option { + if let Some(revs) = Self::wheel_revolutions(a, b) { + let time_delta = b.speed_event_time.wrapping_sub(a.speed_event_time); + return Some(revs as f32 * 1024.0 / time_delta as f32); + } + None + } + + /// Calculates average speed (m/s) + pub fn speed(a: SpeedAndCadence, b: SpeedAndCadence, circumference: f32) -> Option { + if let Some(speed) = Self::speed_revs_per_sec(a, b) { + return Some(speed * circumference); + } + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + use packed_struct::PackedStruct; + + #[test] + fn unpack() { + let raw = [0x09, 0x91, 0xd5, 0x08, 0xd7, 0x90, 0x42, 0x1b]; + let foo = SpeedAndCadence::unpack(&raw).unwrap(); + assert_eq!(foo.cadence_event_time, 37129); + assert_eq!(foo.cadence_revolution_count, 2261); + assert_eq!(foo.speed_event_time, 37079); + assert_eq!(foo.speed_revolution_count, 6978); + } + + #[test] + fn cadence() { + // If the timer hasn't changed we should return None + let a = SpeedAndCadence::new(0, 0, 0, 0); + assert_eq!(SpeedAndCadence::cadence(a, a), None); + + let a = SpeedAndCadence::new(0, 0, 0, 0); + let b = SpeedAndCadence::new(1024, 1, 0, 0); + assert!((SpeedAndCadence::cadence(a, b).unwrap() - 60.0).abs() <= f32::EPSILON); + + // test counter roll-over + let a = SpeedAndCadence::new(u16::MAX, u16::MAX, 0, 0); + let b = SpeedAndCadence::new(1023, 0, 0, 0); + assert!((SpeedAndCadence::cadence(a, b).unwrap() - 60.0).abs() <= f32::EPSILON); + } + + #[test] + fn speed() { + // If the timer hasn't changed we should return None + let a = SpeedAndCadence::new(0, 0, 0, 0); + assert_eq!(SpeedAndCadence::speed(a, a, 1.0), None); + + let a = SpeedAndCadence::new(0, 0, 0, 0); + let b = SpeedAndCadence::new(0, 0, 1024, 1); + assert!((SpeedAndCadence::speed(a, b, 1.0).unwrap() - 1.0).abs() <= f32::EPSILON); + + // test counter roll-over + let a = SpeedAndCadence::new(0, 0, u16::MAX, u16::MAX); + let b = SpeedAndCadence::new(0, 0, 1023, 0); + assert!((SpeedAndCadence::speed(a, b, 1.0).unwrap() - 1.0).abs() <= f32::EPSILON); + } +}