From e9fa40de4aecea437e21eeaaf2cb39f973074072 Mon Sep 17 00:00:00 2001 From: Curtis Malainey Date: Sun, 19 May 2024 14:18:05 -0700 Subject: [PATCH 01/14] msg_handler: add tx_ready test --- ant/src/channel.rs | 1 + ant/src/plus/common/msg_handler.rs | 15 +++++++++++++++ 2 files changed, 16 insertions(+) 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/plus/common/msg_handler.rs b/ant/src/plus/common/msg_handler.rs index f4dd66c..3ba6dbf 100644 --- a/ant/src/plus/common/msg_handler.rs +++ b/ant/src/plus/common/msg_handler.rs @@ -734,4 +734,19 @@ mod tests { fn reset() { // TODO } + + #[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()); + } } From d4b042607a3816fb113fc416bc1c6d607601c24c Mon Sep 17 00:00:00 2001 From: Benjamin Tamasi Date: Thu, 3 Oct 2024 17:02:22 +0200 Subject: [PATCH 02/14] Add Speed & Cadence Profile --- ant/src/plus/profiles/mod.rs | 1 + .../plus/profiles/speed_and_cadence/mod.rs | 119 ++++++++++++++++++ 2 files changed, 120 insertions(+) create mode 100644 ant/src/plus/profiles/speed_and_cadence/mod.rs diff --git a/ant/src/plus/profiles/mod.rs b/ant/src/plus/profiles/mod.rs index 8079913..cb7ed81 100644 --- a/ant/src/plus/profiles/mod.rs +++ b/ant/src/plus/profiles/mod.rs @@ -7,3 +7,4 @@ // except according to those terms. pub mod heart_rate; +pub mod speed_and_cadence; 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..33805b1 --- /dev/null +++ b/ant/src/plus/profiles/speed_and_cadence/mod.rs @@ -0,0 +1,119 @@ +use derive_new::new; +use packed_struct::derive::PackedStruct; + +pub const DEVICE_TYPE: u8 = 121; + +#[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); + } +} From 3714b638456470329ffd44757001a762c35711f1 Mon Sep 17 00:00:00 2001 From: Curtis Malainey Date: Tue, 16 Jul 2024 14:23:35 -0700 Subject: [PATCH 03/14] plus: msg_handler: add some more state tests --- ant/src/plus/common/msg_handler.rs | 42 ++++++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/ant/src/plus/common/msg_handler.rs b/ant/src/plus/common/msg_handler.rs index 3ba6dbf..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,7 +749,28 @@ 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] From 4426dc64b060ad511931644cb419ebd7cb6b8769 Mon Sep 17 00:00:00 2001 From: Curtis Malainey Date: Sun, 13 Apr 2025 14:14:28 -0700 Subject: [PATCH 04/14] remove embedded HAL serial implementation Something is buggy with the serial adapter, its better to use libusb when on a proper host. Fixes: #10 --- ant/Cargo.toml | 3 -- ant/examples/linux_usb.rs | 58 --------------------------------------- 2 files changed, 61 deletions(-) delete mode 100644 ant/examples/linux_usb.rs diff --git a/ant/Cargo.toml b/ant/Cargo.toml index 7fdbbe6..88f23d1 100644 --- a/ant/Cargo.toml +++ b/ant/Cargo.toml @@ -24,9 +24,6 @@ nb = "1.1" rusb = {version = "0.9", 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" diff --git a/ant/examples/linux_usb.rs b/ant/examples/linux_usb.rs deleted file mode 100644 index aa87c4a..0000000 --- a/ant/examples/linux_usb.rs +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2023 Google LLC -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -use ant::drivers::*; -use ant::messages::config::{ - AssignChannel, ChannelId, ChannelPeriod, ChannelRfFrequency, ChannelType, DeviceType, - LibConfig, SetNetworkKey, TransmissionType, -}; -use ant::messages::control::{OpenChannel, ResetSystem}; - -use linux_embedded_hal::Serial; - -use std::env; - -fn main() -> std::io::Result<()> { - let args: Vec = env::args().collect(); - if args.len() != 1 { - panic!("Expected single arguement TTY") - } - let serial = Serial::open(args[0].clone(), 115200).expect("Serial failed to open"); - let mut driver = SerialDriver::<_, StubPin>::new(serial, None); - let assign = AssignChannel::new(0, ChannelType::BidirectionalSlave, 0, None); - let key = SetNetworkKey::new(0, [0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77]); // get this - // from - // thisisant.com - let rf = ChannelRfFrequency::new(0, 57); - let id = ChannelId::new( - 0, - 0, - DeviceType::new(120.into(), false), - TransmissionType::new_wildcard(), - ); - let period = ChannelPeriod::new(0, 8070); - 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(&OpenChannel::new(0)) - .expect("Message failed"); - loop { - match driver.get_message() { - Ok(None) => (), - msg => println!("{:#?}", msg), - } - } -} From e328f61fc834fdc8707e4b46a2de6b30d63144b0 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Mon, 29 Sep 2025 20:42:16 +0200 Subject: [PATCH 05/14] feat: added FE-C profile & example --- ant/examples/linux_usb_fec_display.rs | 144 ++++++++++ .../fitness_equipment_controls/datapages.rs | 84 ++++++ .../fitness_equipment_controls/display.rs | 250 ++++++++++++++++++ .../fitness_equipment_controls/mod.rs | 90 +++++++ ant/src/plus/profiles/mod.rs | 1 + 5 files changed, 569 insertions(+) create mode 100644 ant/examples/linux_usb_fec_display.rs create mode 100644 ant/src/plus/profiles/fitness_equipment_controls/datapages.rs create mode 100644 ant/src/plus/profiles/fitness_equipment_controls/display.rs create mode 100644 ant/src/plus/profiles/fitness_equipment_controls/mod.rs diff --git a/ant/examples/linux_usb_fec_display.rs b/ant/examples/linux_usb_fec_display.rs new file mode 100644 index 0000000..ae63fce --- /dev/null +++ b/ant/examples/linux_usb_fec_display.rs @@ -0,0 +1,144 @@ +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::data::AcknowledgedData; +use ant::messages::{RxMessage, TxMessage}; +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; + + 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) { + let target_power = 50 * resistance_multiplier * 4; + println!("Setting target power to {} : {}", (target_power & 0xFF) as u8, (target_power >> 8) as u8); + let message: TxMessage = AcknowledgedData::new(chan, [ + 0x31, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + (target_power & 0xFF) as u8, + (target_power >> 8) as u8, + ]).into(); + router.send(&message).expect("Failed to send message"); + println!("Sent target power: {} watts", target_power / 4); + resistance_multiplier += 1; + last_send_time = Instant::now(); + } + } +} 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..752ab60 --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs @@ -0,0 +1,84 @@ +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(bits = "1:7")] + data_page_number: Integer>, + #[packed_field(bits = "0")] + pub page_change_toggle: bool, + #[packed_field(bytes = "1")] + pub event_count: u8, + #[packed_field(bytes = "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(bytes = "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 BasicResistanceDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:6")] + pub reserved: [u8; 6], + #[packed_field(bytes = "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")] + pub reserved: [u8; 5], + #[packed_field(bytes = "6")] + pub total_power_lsb: u8, + #[packed_field(bytes = "7")] + pub total_power_rsb: u8, +} \ No newline at end of file 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..05ac58e --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -0,0 +1,250 @@ +use crate::channel::{self, 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::fitness_equipment_controls::{ + DataPageNumbers, EquipmentType, Error, MainDataPage, MonitorTxDataPage, + Period, PowerDataPage, 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, + equipment_type: Option, + virtual_speed: Option, + real_speed: Option, + elapsed_time: u16, + distance: u16, +} + +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, + } + } + + 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 => + MonitorTxDataPage::PowerDataPage(PowerDataPage::unpack(data)?), + }; + 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 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/fitness_equipment_controls/mod.rs b/ant/src/plus/profiles/fitness_equipment_controls/mod.rs new file mode 100644 index 0000000..833db13 --- /dev/null +++ b/ant/src/plus/profiles/fitness_equipment_controls/mod.rs @@ -0,0 +1,90 @@ +//! 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), + 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, + } + } +} \ No newline at end of file diff --git a/ant/src/plus/profiles/mod.rs b/ant/src/plus/profiles/mod.rs index cb7ed81..04bb731 100644 --- a/ant/src/plus/profiles/mod.rs +++ b/ant/src/plus/profiles/mod.rs @@ -8,3 +8,4 @@ pub mod heart_rate; pub mod speed_and_cadence; +pub mod fitness_equipment_controls; \ No newline at end of file From 719d2021e12664c629187635cc8991534bc16f67 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Mon, 29 Sep 2025 20:45:05 +0200 Subject: [PATCH 06/14] docs: removed section --- README.md | 5 ----- 1 file changed, 5 deletions(-) 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 From 2c526e8027d097b0599fcdeec41fbd50907d74f3 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Thu, 16 Oct 2025 14:26:45 +0200 Subject: [PATCH 07/14] feat: added linux_usb_multi_channel example && added speed_and_cadence display --- ant/examples/linux_usb_multi_channel.rs | 230 ++++++++++++++++++ .../fitness_equipment_controls/display.rs | 2 +- ant/src/plus/profiles/heart_rate/.mod.rs.swp | Bin 4096 -> 0 bytes .../profiles/speed_and_cadence/datapages.rs | 24 ++ .../profiles/speed_and_cadence/display.rs | 190 +++++++++++++++ .../plus/profiles/speed_and_cadence/mod.rs | 57 +++++ 6 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 ant/examples/linux_usb_multi_channel.rs delete mode 100644 ant/src/plus/profiles/heart_rate/.mod.rs.swp create mode 100644 ant/src/plus/profiles/speed_and_cadence/datapages.rs create mode 100644 ant/src/plus/profiles/speed_and_cadence/display.rs diff --git a/ant/examples/linux_usb_multi_channel.rs b/ant/examples/linux_usb_multi_channel.rs new file mode 100644 index 0000000..ad2415d --- /dev/null +++ b/ant/examples/linux_usb_multi_channel.rs @@ -0,0 +1,230 @@ +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::{AntMessage, RxMessage, TxMessage}; +use ant::plus::profiles::{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 chan = router + // .add_channel(TxSender { sender: router_tx }) + // .expect("Add channel failed"); + // let tacx_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( + // tacx_config, + // TxSender { sender: channel_tx.clone() }, + // 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!("17: {:x?}", x.payload.channel_number), + // _ => {} + // } + // })); + + // tacx.open(); + + // let (router_tx, channel_rx) = channel(8); + // let chan = router + // .add_channel(TxSender { sender: router_tx }) + // .expect("Add channel failed"); + // let tacx_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( + // 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!("17: {:x?}", x.payload.channel_number), + // _ => {} + // } + // })); + + // tacx.open(); + + let mut tacx = setup_fec_channel(&mut router, channel_tx.clone()); + let mut tacx2 = setup_sac_channel(&mut router, channel_tx.clone()); + + loop { + router.process().unwrap(); + tacx.process().unwrap(); + tacx2.process().unwrap(); + } +} + +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: 0, + 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| { + 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 +} \ No newline at end of file diff --git a/ant/src/plus/profiles/fitness_equipment_controls/display.rs b/ant/src/plus/profiles/fitness_equipment_controls/display.rs index 05ac58e..0c8588f 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/display.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -1,4 +1,4 @@ -use crate::channel::{self, duration_to_search_timeout, TxError}; +use crate::channel::{duration_to_search_timeout, TxError}; use crate::channel::{ChanError, RxHandler, TxHandler}; use crate::messages::config::{ ChannelType, TransmissionChannelType, TransmissionGlobalDataPages, TransmissionType 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 1b4d2c531c6a48ea82435a4d601282eb026d2d35..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4096 zcmYc?2=nw+u+%eP00IF92E(?`DX)$mWi_y1WJoS8D#jr4NzlM{1LG}NIQsPCRxl2MwZpO{yoTT~39^oxs<^$T)Ji}eeN z^3yVNQj7I7QWJ|x;)@bXQuTB5Q}l|8QA`}=jE2By2oN6vybQ)hhTyEOtfZ(QEEGz- j)=`zCAut*OqaiRF0;3@?8UmvsFd71*Aut*OLp}rmre8B$ 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 index 33805b1..5ed7703 100644 --- a/ant/src/plus/profiles/speed_and_cadence/mod.rs +++ b/ant/src/plus/profiles/speed_and_cadence/mod.rs @@ -1,8 +1,65 @@ +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 { From fdc4e082507c08c114916df58e65dbe0a6425a2e Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Thu, 16 Oct 2025 15:33:59 +0200 Subject: [PATCH 08/14] feat: added set_target_power to fec display --- ant/examples/linux_usb_fec_display.rs | 21 +++++++------------ .../fitness_equipment_controls/display.rs | 15 +++++++++++-- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/ant/examples/linux_usb_fec_display.rs b/ant/examples/linux_usb_fec_display.rs index ae63fce..23023db 100644 --- a/ant/examples/linux_usb_fec_display.rs +++ b/ant/examples/linux_usb_fec_display.rs @@ -123,20 +123,13 @@ fn main() -> std::io::Result<()> { // Every 5 seconds, increase the target power by 50 watts if last_send_time.elapsed() >= Duration::from_secs(5) { - let target_power = 50 * resistance_multiplier * 4; - println!("Setting target power to {} : {}", (target_power & 0xFF) as u8, (target_power >> 8) as u8); - let message: TxMessage = AcknowledgedData::new(chan, [ - 0x31, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - (target_power & 0xFF) as u8, - (target_power >> 8) as u8, - ]).into(); - router.send(&message).expect("Failed to send message"); - println!("Sent target power: {} watts", target_power / 4); + 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/src/plus/profiles/fitness_equipment_controls/display.rs b/ant/src/plus/profiles/fitness_equipment_controls/display.rs index 0c8588f..6117fed 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/display.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -3,6 +3,7 @@ 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}; @@ -242,8 +243,18 @@ impl, R: RxHandler> Display { self.distance } - pub fn direct_send(&self, message: TxMessage) -> Result<(), TxError> { - println!("Sending message: {:?}", message); + pub fn set_power_target(&mut self, power: u16) -> Result<(), TxError> { + let power: u16 = power * 4; + let message: TxMessage = AcknowledgedData::new(0, [ + 0x31, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + (power & 0xFF) as u8, + (power >> 8) as u8, + ]).into(); self.tx.try_send(message)?; Ok(()) } From 6e8ce71cda469eea952bfa45852e92f80c1cce50 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Thu, 16 Oct 2025 15:34:24 +0200 Subject: [PATCH 09/14] chore: removed imports --- ant/examples/linux_usb_fec_display.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ant/examples/linux_usb_fec_display.rs b/ant/examples/linux_usb_fec_display.rs index 23023db..daafc06 100644 --- a/ant/examples/linux_usb_fec_display.rs +++ b/ant/examples/linux_usb_fec_display.rs @@ -4,8 +4,7 @@ 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::data::AcknowledgedData; -use ant::messages::{RxMessage, TxMessage}; +use ant::messages::RxMessage; use ant::plus::profiles::fitness_equipment_controls::{Display, DisplayConfig, Period}; use ant::router::Router; use dialoguer::Select; From d2fbaa37e9bbe18d4efe697a7540576eb055adb6 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Fri, 17 Oct 2025 09:17:02 +0200 Subject: [PATCH 10/14] feat: added discovery profile --- ant/examples/linux_usb_multi_channel.rs | 109 ++++++--------- ant/src/messages/control.rs | 6 + ant/src/messages/mod.rs | 10 +- ant/src/plus/profiles/discovery/datapages.rs | 84 +++++++++++ ant/src/plus/profiles/discovery/display.rs | 140 +++++++++++++++++++ ant/src/plus/profiles/discovery/mod.rs | 31 ++++ ant/src/plus/profiles/mod.rs | 3 +- ant/src/router.rs | 1 + 8 files changed, 312 insertions(+), 72 deletions(-) create mode 100644 ant/src/plus/profiles/discovery/datapages.rs create mode 100644 ant/src/plus/profiles/discovery/display.rs create mode 100644 ant/src/plus/profiles/discovery/mod.rs diff --git a/ant/examples/linux_usb_multi_channel.rs b/ant/examples/linux_usb_multi_channel.rs index ad2415d..54a6cae 100644 --- a/ant/examples/linux_usb_multi_channel.rs +++ b/ant/examples/linux_usb_multi_channel.rs @@ -3,7 +3,7 @@ use ant::drivers::{is_ant_usb_device_from_device, UsbDriver}; use ant::messages::channel::MessageCode; use ant::messages::config::SetNetworkKey; use ant::messages::{AntMessage, RxMessage, TxMessage}; -use ant::plus::profiles::{fitness_equipment_controls, speed_and_cadence}; +use ant::plus::profiles::{discovery, fitness_equipment_controls, speed_and_cadence}; use ant::router::Router; use dialoguer::Select; use rusb::{Device, DeviceList}; @@ -82,74 +82,16 @@ fn main() -> std::io::Result<()> { .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 tacx_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( - // tacx_config, - // TxSender { sender: channel_tx.clone() }, - // 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!("17: {:x?}", x.payload.channel_number), - // _ => {} - // } - // })); - - // tacx.open(); - - // let (router_tx, channel_rx) = channel(8); - // let chan = router - // .add_channel(TxSender { sender: router_tx }) - // .expect("Add channel failed"); - // let tacx_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( - // 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!("17: {:x?}", x.payload.channel_number), - // _ => {} - // } - // })); - - // tacx.open(); - - let mut tacx = setup_fec_channel(&mut router, channel_tx.clone()); - let mut tacx2 = setup_sac_channel(&mut router, channel_tx.clone()); + + // let mut tacx = setup_fec_channel(&mut router, channel_tx.clone()); + // let mut tacx2 = setup_sac_channel(&mut router, channel_tx.clone()); + let mut discovery = setup_discovery_channel(&mut router, channel_tx.clone()); loop { router.process().unwrap(); - tacx.process().unwrap(); - tacx2.process().unwrap(); + // tacx.process().unwrap(); + // tacx2.process().unwrap(); + discovery.process().unwrap(); } } @@ -226,5 +168,40 @@ fn setup_sac_channel( 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| { + 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!("Discovery: {:#?}", x), + _ => {} + } + })); + + tacx.open(); + tacx } \ No newline at end of file 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..8c68db7 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/plus/profiles/discovery/datapages.rs b/ant/src/plus/profiles/discovery/datapages.rs new file mode 100644 index 0000000..752ab60 --- /dev/null +++ b/ant/src/plus/profiles/discovery/datapages.rs @@ -0,0 +1,84 @@ +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(bits = "1:7")] + data_page_number: Integer>, + #[packed_field(bits = "0")] + pub page_change_toggle: bool, + #[packed_field(bytes = "1")] + pub event_count: u8, + #[packed_field(bytes = "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(bytes = "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 BasicResistanceDataPage { + #[packed_field(byte = "0")] + data_page_number: u8, + #[packed_field(bytes = "1:6")] + pub reserved: [u8; 6], + #[packed_field(bytes = "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")] + pub reserved: [u8; 5], + #[packed_field(bytes = "6")] + pub total_power_lsb: u8, + #[packed_field(bytes = "7")] + pub total_power_rsb: u8, +} \ No newline at end of file diff --git a/ant/src/plus/profiles/discovery/display.rs b/ant/src/plus/profiles/discovery/display.rs new file mode 100644 index 0000000..b9fbe5e --- /dev/null +++ b/ant/src/plus/profiles/discovery/display.rs @@ -0,0 +1,140 @@ +use crate::channel::{duration_to_search_timeout}; +use crate::channel::{ChanError, RxHandler, TxHandler}; +use crate::messages::config::{ + ChannelType, LibConfig, TransmissionType +}; +use crate::messages::control::{CloseChannel, OpenRxScanMode}; +use crate::messages::{AntMessage, 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::{ + Error, MonitorTxDataPage +}; +use crate::plus::NETWORK_RF_FREQUENCY; + +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, + is_scanning: bool, +} + +pub struct DisplayConfig { + pub channel: u8, + pub ant_plus_key_index: u8, +} + +impl, R: RxHandler> Display { + pub fn new( + conf: DisplayConfig, + tx: T, + rx: R, + ) -> Self { + let channel_config = ChannelConfig { + channel: conf.channel, + device_number: 0, + device_type: 0, + channel_type: ChannelType::BidirectionalSlave, + network_key_index: conf.ant_plus_key_index, + transmission_type: TransmissionType::new_wildcard(), + radio_frequency: NETWORK_RF_FREQUENCY, + timeout_duration: duration_to_search_timeout(Duration::from_secs(30)), + channel_period: 8192, + }; + 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, + is_scanning: false, + } + } + + pub fn open(&mut self) { + self.msg_handler.open(); + } + + pub fn close(&mut self) { + self.msg_handler.close(); + self.is_scanning = false; + } + + 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 + } + + pub fn process(&mut self) -> Result<(), ChanError> { + if self.msg_handler.is_tx_ready() && !self.is_scanning { // Start scanning instead of opening channel + self.tx.try_send(CloseChannel::new(self.msg_handler.get_channel()).into()).unwrap(); + self.tx.try_send(LibConfig::new(true, false, false).into()).unwrap(); + self.tx.try_send(OpenRxScanMode::new(Some(false)).into()).unwrap(); + self.is_scanning = true; + } + + // TODO handle closed channel + while let Ok(msg) = self.rx.try_recv() { + if let Some(f) = self.rx_message_callback { + f(&msg); + } + 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() { + msg.set_channel(self.msg_handler.get_channel()); + self.msg_handler.tx_sent(); + self.tx.try_send(msg.into())?; + } + } + } + Ok(()) + } +} \ No newline at end of file diff --git a/ant/src/plus/profiles/discovery/mod.rs b/ant/src/plus/profiles/discovery/mod.rs new file mode 100644 index 0000000..cdf58de --- /dev/null +++ b/ant/src/plus/profiles/discovery/mod.rs @@ -0,0 +1,31 @@ +//! 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; + +#[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) + } +} diff --git a/ant/src/plus/profiles/mod.rs b/ant/src/plus/profiles/mod.rs index 04bb731..167310b 100644 --- a/ant/src/plus/profiles/mod.rs +++ b/ant/src/plus/profiles/mod.rs @@ -8,4 +8,5 @@ pub mod heart_rate; pub mod speed_and_cadence; -pub mod fitness_equipment_controls; \ No newline at end of file +pub mod fitness_equipment_controls; +pub mod discovery; \ No newline at end of file diff --git a/ant/src/router.rs b/ant/src/router.rs index 5e7a699..74d8cd0 100644 --- a/ant/src/router.rs +++ b/ant/src/router.rs @@ -227,6 +227,7 @@ impl, T: TxHandler, R: RxHandler> Router< self.handle_message(msg)?; } while let Ok(msg) = self.receiver.try_recv() { + println!("Router sending message: {:?}", msg); self.driver.send_message(&msg)?; } Ok(()) From e2decfb3935a129e3c14ddec77002ca894034444 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Tue, 21 Oct 2025 16:37:58 +0200 Subject: [PATCH 11/14] fix: remove discovery init messages --- ant/examples/linux_usb_multi_channel.rs | 66 +++++++++++++++------- ant/src/plus/profiles/discovery/display.rs | 16 +----- 2 files changed, 50 insertions(+), 32 deletions(-) diff --git a/ant/examples/linux_usb_multi_channel.rs b/ant/examples/linux_usb_multi_channel.rs index 54a6cae..94af232 100644 --- a/ant/examples/linux_usb_multi_channel.rs +++ b/ant/examples/linux_usb_multi_channel.rs @@ -1,7 +1,13 @@ +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::SetNetworkKey; +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; @@ -83,15 +89,28 @@ fn main() -> std::io::Result<()> { let snk = SetNetworkKey::new(0, [0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x45]); router.send(&snk).expect("failed to set network key"); - // let mut tacx = setup_fec_channel(&mut router, channel_tx.clone()); - // let mut tacx2 = setup_sac_channel(&mut router, channel_tx.clone()); 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 { - router.process().unwrap(); - // tacx.process().unwrap(); - // tacx2.process().unwrap(); + 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; + } } } @@ -104,7 +123,7 @@ fn setup_fec_channel( .add_channel(TxSender { sender: router_tx }) .expect("Add channel failed"); let tacx_config = fitness_equipment_controls::DisplayConfig { - device_number: 0, + device_number: 9609, device_number_extension: 0.into(), channel: chan, period: fitness_equipment_controls::Period::FourHz, @@ -116,16 +135,17 @@ fn setup_fec_channel( 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!("17: {:x?}", x.payload.channel_number), - _ => {} - } + 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(); @@ -189,14 +209,22 @@ fn setup_discovery_channel( 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!("Discovery: {:#?}", x), + 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), _ => {} } })); diff --git a/ant/src/plus/profiles/discovery/display.rs b/ant/src/plus/profiles/discovery/display.rs index b9fbe5e..af9f02d 100644 --- a/ant/src/plus/profiles/discovery/display.rs +++ b/ant/src/plus/profiles/discovery/display.rs @@ -1,9 +1,9 @@ use crate::channel::{duration_to_search_timeout}; use crate::channel::{ChanError, RxHandler, TxHandler}; use crate::messages::config::{ - ChannelType, LibConfig, TransmissionType + ChannelType, TransmissionType }; -use crate::messages::control::{CloseChannel, OpenRxScanMode}; +use crate::messages::control::{RequestMessage, RequestableMessageId}; use crate::messages::{AntMessage, TxMessage, TxMessageChannelConfig, TxMessageData}; // use crate::plus::common::datapages::MANUFACTURER_SPECIFIC_RANGE; use crate::plus::common::msg_handler::{ChannelConfig, MessageHandler}; @@ -22,7 +22,6 @@ pub struct Display, R: RxHandler> { tx_datapage_callback: Option Option>, tx: T, rx: R, - is_scanning: bool, } pub struct DisplayConfig { @@ -55,7 +54,6 @@ impl, R: RxHandler> Display { msg_handler: MessageHandler::new(&channel_config), tx, rx, - is_scanning: false, } } @@ -65,7 +63,6 @@ impl, R: RxHandler> Display { pub fn close(&mut self) { self.msg_handler.close(); - self.is_scanning = false; } pub fn get_device_id(&self) -> u16 { @@ -93,14 +90,7 @@ impl, R: RxHandler> Display { } pub fn process(&mut self) -> Result<(), ChanError> { - if self.msg_handler.is_tx_ready() && !self.is_scanning { // Start scanning instead of opening channel - self.tx.try_send(CloseChannel::new(self.msg_handler.get_channel()).into()).unwrap(); - self.tx.try_send(LibConfig::new(true, false, false).into()).unwrap(); - self.tx.try_send(OpenRxScanMode::new(Some(false)).into()).unwrap(); - self.is_scanning = true; - } - - // TODO handle closed channel + // TODO handle closed channel while let Ok(msg) = self.rx.try_recv() { if let Some(f) = self.rx_message_callback { f(&msg); From 4a00a616d81be0718db39008a9d2b86140575dc8 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Sun, 2 Nov 2025 09:59:59 +0100 Subject: [PATCH 12/14] fix: set channel --- .../plus/profiles/fitness_equipment_controls/display.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ant/src/plus/profiles/fitness_equipment_controls/display.rs b/ant/src/plus/profiles/fitness_equipment_controls/display.rs index 6117fed..3a19645 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/display.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -8,7 +8,7 @@ use crate::messages::{AntMessage, RxMessage, TxMessage, TxMessageChannelConfig, // use crate::plus::common::datapages::MANUFACTURER_SPECIFIC_RANGE; use crate::plus::common::msg_handler::{ChannelConfig, MessageHandler}; use crate::plus::profiles::fitness_equipment_controls::{ - DataPageNumbers, EquipmentType, Error, MainDataPage, MonitorTxDataPage, + DataPageNumbers, EquipmentType, Error, MainDataPage, MonitorTxDataPage, Period, PowerDataPage, DATA_PAGE_NUMBER_MASK, DEVICE_TYPE }; use crate::plus::NETWORK_RF_FREQUENCY; @@ -162,7 +162,7 @@ impl, R: RxHandler> Display { MonitorTxDataPage::MainDataPage(page) }, - DataPageNumbers::PowerDataPage => + DataPageNumbers::PowerDataPage => MonitorTxDataPage::PowerDataPage(PowerDataPage::unpack(data)?), }; return Ok(parsed); @@ -245,7 +245,7 @@ impl, R: RxHandler> Display { pub fn set_power_target(&mut self, power: u16) -> Result<(), TxError> { let power: u16 = power * 4; - let message: TxMessage = AcknowledgedData::new(0, [ + let message: TxMessage = AcknowledgedData::new(self.msg_handler.get_channel(), [ 0x31, 0x00, 0x00, @@ -258,4 +258,4 @@ impl, R: RxHandler> Display { self.tx.try_send(message)?; Ok(()) } -} \ No newline at end of file +} From 89e627e7e629178a0ead7bdd7c625cacfb2e0d0d Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Sun, 2 Nov 2025 10:12:40 +0100 Subject: [PATCH 13/14] fix: set channel --- ant/src/plus/profiles/fitness_equipment_controls/display.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ant/src/plus/profiles/fitness_equipment_controls/display.rs b/ant/src/plus/profiles/fitness_equipment_controls/display.rs index 3a19645..90b95a7 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/display.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -245,7 +245,7 @@ impl, R: RxHandler> Display { pub fn set_power_target(&mut self, power: u16) -> Result<(), TxError> { let power: u16 = power * 4; - let message: TxMessage = AcknowledgedData::new(self.msg_handler.get_channel(), [ + let mut message: TxMessageData = AcknowledgedData::new(0, [ 0x31, 0x00, 0x00, @@ -255,7 +255,8 @@ impl, R: RxHandler> Display { (power & 0xFF) as u8, (power >> 8) as u8, ]).into(); - self.tx.try_send(message)?; + message.set_channel(self.msg_handler.get_channel()); + self.tx.try_send(message.into())?; Ok(()) } } From 0ff88ff4cdbfee4a255cc2bb5e649da6572c2413 Mon Sep 17 00:00:00 2001 From: Koert Weber Date: Sun, 22 Mar 2026 15:32:14 +0100 Subject: [PATCH 14/14] feat: added cadence, set_user_configuration, set_basic_resistance, set_wind_resistance, set_track_resistance to the fec display --- ant/Cargo.toml | 3 +- ant/examples/linux_usb.rs | 58 +++++ ant/examples/linux_usb_discovery.rs | 87 +++++++ ant/examples/linux_usb_fec_display.rs | 9 + ant/examples/linux_usb_router_discovery.rs | 239 ++++++++++++++++++ ant/src/drivers/usb.rs | 4 +- ant/src/messages/mod.rs | 4 +- ant/src/messages/requested_response.rs | 2 +- ant/src/plus/profiles/discovery/datapages.rs | 84 ------ ant/src/plus/profiles/discovery/display.rs | 130 ---------- ant/src/plus/profiles/discovery/mod.rs | 31 --- .../fitness_equipment_controls/datapages.rs | 71 +++++- .../fitness_equipment_controls/display.rs | 134 +++++++++- .../fitness_equipment_controls/mod.rs | 6 +- ant/src/router.rs | 1 - 15 files changed, 583 insertions(+), 280 deletions(-) create mode 100644 ant/examples/linux_usb.rs create mode 100644 ant/examples/linux_usb_discovery.rs create mode 100644 ant/examples/linux_usb_router_discovery.rs delete mode 100644 ant/src/plus/profiles/discovery/datapages.rs delete mode 100644 ant/src/plus/profiles/discovery/display.rs delete mode 100644 ant/src/plus/profiles/discovery/mod.rs diff --git a/ant/Cargo.toml b/ant/Cargo.toml index 88f23d1..a7c3287 100644 --- a/ant/Cargo.toml +++ b/ant/Cargo.toml @@ -21,7 +21,7 @@ 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} [dev-dependencies] @@ -38,4 +38,3 @@ usb_adapter = ["dep:rusb", "usb"] [[test]] name = "serial" - diff --git a/ant/examples/linux_usb.rs b/ant/examples/linux_usb.rs new file mode 100644 index 0000000..aa87c4a --- /dev/null +++ b/ant/examples/linux_usb.rs @@ -0,0 +1,58 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 or the MIT license +// , at your +// option. This file may not be copied, modified, or distributed +// except according to those terms. + +use ant::drivers::*; +use ant::messages::config::{ + AssignChannel, ChannelId, ChannelPeriod, ChannelRfFrequency, ChannelType, DeviceType, + LibConfig, SetNetworkKey, TransmissionType, +}; +use ant::messages::control::{OpenChannel, ResetSystem}; + +use linux_embedded_hal::Serial; + +use std::env; + +fn main() -> std::io::Result<()> { + let args: Vec = env::args().collect(); + if args.len() != 1 { + panic!("Expected single arguement TTY") + } + let serial = Serial::open(args[0].clone(), 115200).expect("Serial failed to open"); + let mut driver = SerialDriver::<_, StubPin>::new(serial, None); + let assign = AssignChannel::new(0, ChannelType::BidirectionalSlave, 0, None); + let key = SetNetworkKey::new(0, [0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77]); // get this + // from + // thisisant.com + let rf = ChannelRfFrequency::new(0, 57); + let id = ChannelId::new( + 0, + 0, + DeviceType::new(120.into(), false), + TransmissionType::new_wildcard(), + ); + let period = ChannelPeriod::new(0, 8070); + 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(&OpenChannel::new(0)) + .expect("Message failed"); + loop { + match driver.get_message() { + Ok(None) => (), + msg => println!("{:#?}", msg), + } + } +} 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 index daafc06..da28f8d 100644 --- a/ant/examples/linux_usb_fec_display.rs +++ b/ant/examples/linux_usb_fec_display.rs @@ -115,6 +115,7 @@ fn main() -> std::io::Result<()> { let mut last_send_time = Instant::now(); let mut resistance_multiplier = 2; + let mut has_set_init_values = false; loop { router.process().unwrap(); @@ -122,6 +123,14 @@ fn main() -> std::io::Result<()> { // 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); 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/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/mod.rs b/ant/src/messages/mod.rs index 8c68db7..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}; +}, control::OpenRxScanMode}}; use channel::{ChannelEvent, ChannelResponse}; use control::{CloseChannel, OpenChannel, RequestMessage, ResetSystem, SleepMessage}; use data::{ 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/profiles/discovery/datapages.rs b/ant/src/plus/profiles/discovery/datapages.rs deleted file mode 100644 index 752ab60..0000000 --- a/ant/src/plus/profiles/discovery/datapages.rs +++ /dev/null @@ -1,84 +0,0 @@ -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(bits = "1:7")] - data_page_number: Integer>, - #[packed_field(bits = "0")] - pub page_change_toggle: bool, - #[packed_field(bytes = "1")] - pub event_count: u8, - #[packed_field(bytes = "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(bytes = "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 BasicResistanceDataPage { - #[packed_field(byte = "0")] - data_page_number: u8, - #[packed_field(bytes = "1:6")] - pub reserved: [u8; 6], - #[packed_field(bytes = "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")] - pub reserved: [u8; 5], - #[packed_field(bytes = "6")] - pub total_power_lsb: u8, - #[packed_field(bytes = "7")] - pub total_power_rsb: u8, -} \ No newline at end of file diff --git a/ant/src/plus/profiles/discovery/display.rs b/ant/src/plus/profiles/discovery/display.rs deleted file mode 100644 index af9f02d..0000000 --- a/ant/src/plus/profiles/discovery/display.rs +++ /dev/null @@ -1,130 +0,0 @@ -use crate::channel::{duration_to_search_timeout}; -use crate::channel::{ChanError, RxHandler, TxHandler}; -use crate::messages::config::{ - ChannelType, TransmissionType -}; -use crate::messages::control::{RequestMessage, RequestableMessageId}; -use crate::messages::{AntMessage, 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::{ - Error, MonitorTxDataPage -}; -use crate::plus::NETWORK_RF_FREQUENCY; - -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 ant_plus_key_index: u8, -} - -impl, R: RxHandler> Display { - pub fn new( - conf: DisplayConfig, - tx: T, - rx: R, - ) -> Self { - let channel_config = ChannelConfig { - channel: conf.channel, - device_number: 0, - device_type: 0, - channel_type: ChannelType::BidirectionalSlave, - network_key_index: conf.ant_plus_key_index, - transmission_type: TransmissionType::new_wildcard(), - radio_frequency: NETWORK_RF_FREQUENCY, - timeout_duration: duration_to_search_timeout(Duration::from_secs(30)), - channel_period: 8192, - }; - 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 - } - - 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 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() { - msg.set_channel(self.msg_handler.get_channel()); - self.msg_handler.tx_sent(); - self.tx.try_send(msg.into())?; - } - } - } - Ok(()) - } -} \ No newline at end of file diff --git a/ant/src/plus/profiles/discovery/mod.rs b/ant/src/plus/profiles/discovery/mod.rs deleted file mode 100644 index cdf58de..0000000 --- a/ant/src/plus/profiles/discovery/mod.rs +++ /dev/null @@ -1,31 +0,0 @@ -//! 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; - -#[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) - } -} diff --git a/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs b/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs index 752ab60..51f68bf 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/datapages.rs @@ -41,13 +41,13 @@ pub struct MainDataPage { #[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(bits = "0")] - pub page_change_toggle: bool, - #[packed_field(bytes = "1")] + #[packed_field(byte = "1")] pub event_count: u8, - #[packed_field(bytes = "2")] + #[packed_field(byte = "2")] pub cadance: u8, #[packed_field(bytes = "3:4")] pub accumulated_power: u16, @@ -55,18 +55,65 @@ pub struct PowerDataPage { pub instantaneous_power: Integer>, #[packed_field(bits = "52:55")] pub trainer_status: u8, - #[packed_field(bytes = "7")] + #[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")] - pub reserved: [u8; 6], - #[packed_field(bytes = "7")] + _reserved: [u8; 6], + #[packed_field(byte = "7")] pub total_resistance: u8, } @@ -76,9 +123,7 @@ pub struct TargetPowerDataPage { #[packed_field(byte = "0")] data_page_number: u8, #[packed_field(bytes = "1:5")] - pub reserved: [u8; 5], - #[packed_field(bytes = "6")] - pub total_power_lsb: u8, - #[packed_field(bytes = "7")] - pub total_power_rsb: u8, -} \ No newline at end of file + _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 index 90b95a7..dde1e4d 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/display.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/display.rs @@ -8,10 +8,10 @@ use crate::messages::{AntMessage, RxMessage, TxMessage, TxMessageChannelConfig, // use crate::plus::common::datapages::MANUFACTURER_SPECIFIC_RANGE; use crate::plus::common::msg_handler::{ChannelConfig, MessageHandler}; use crate::plus::profiles::fitness_equipment_controls::{ - DataPageNumbers, EquipmentType, Error, MainDataPage, MonitorTxDataPage, - Period, PowerDataPage, DATA_PAGE_NUMBER_MASK, DEVICE_TYPE + 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}; @@ -31,6 +31,10 @@ pub struct Display, R: RxHandler> { real_speed: Option, elapsed_time: u16, distance: u16, + cadence: u8, + power: u16, + last_power: u16, + power_event_count: u8, } pub struct DisplayConfig { @@ -80,6 +84,10 @@ impl, R: RxHandler> Display { real_speed: None, elapsed_time: 0, distance: 0, + cadence: 0, + power: 0, + last_power: 0, + power_event_count: 0, } } @@ -162,8 +170,35 @@ impl, R: RxHandler> Display { MonitorTxDataPage::MainDataPage(page) }, - DataPageNumbers::PowerDataPage => - MonitorTxDataPage::PowerDataPage(PowerDataPage::unpack(data)?), + 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); } @@ -243,18 +278,91 @@ impl, R: RxHandler> Display { 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, [ + let mut message: TxMessageData = AcknowledgedData::new(0, TargetPowerDataPage::new( 0x31, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - (power & 0xFF) as u8, - (power >> 8) as u8, - ]).into(); + [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 index 833db13..68a2b3b 100644 --- a/ant/src/plus/profiles/fitness_equipment_controls/mod.rs +++ b/ant/src/plus/profiles/fitness_equipment_controls/mod.rs @@ -38,6 +38,10 @@ pub enum MonitorTxDataPage { #[derive(PartialEq, Copy, Clone, Debug)] pub enum DisplayTxDataPage { // ManufacturerSpecific(ManufacturerSpecific), + UserConfigurationDataPage(UserConfigurationDataPage), + BasicResistanceDataPage(BasicResistanceDataPage), + WindResistanceDataPage(WindResistanceDataPage), + TrackResistanceDataPage(TrackResistanceDataPage), TargetPowerDataPage(TargetPowerDataPage), } @@ -87,4 +91,4 @@ impl From for EquipmentType { _ => EquipmentType::General, } } -} \ No newline at end of file +} diff --git a/ant/src/router.rs b/ant/src/router.rs index 74d8cd0..5e7a699 100644 --- a/ant/src/router.rs +++ b/ant/src/router.rs @@ -227,7 +227,6 @@ impl, T: TxHandler, R: RxHandler> Router< self.handle_message(msg)?; } while let Ok(msg) = self.receiver.try_recv() { - println!("Router sending message: {:?}", msg); self.driver.send_message(&msg)?; } Ok(())