From 74ad6509fd04166864801343e6b15a27d3fd04f3 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 14:56:07 +0000 Subject: [PATCH 01/31] Add slimmed down DC example to start with --- examples/ds402.rs | 283 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 283 insertions(+) create mode 100644 examples/ds402.rs diff --git a/examples/ds402.rs b/examples/ds402.rs new file mode 100644 index 00000000..0e1982f5 --- /dev/null +++ b/examples/ds402.rs @@ -0,0 +1,283 @@ +//! Configure Distributed Clocks (DC) for EK1100 and a couple of other modules. +//! +//! Please note this example uses experimental features and should not be used as a reference for +//! other code. It is here (currently) primarily to help develop EtherCrab. + +use env_logger::Env; +use ethercrab::{ + error::Error, + std::ethercat_now, + subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, + DcSync, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, Timeouts, +}; +use futures_lite::StreamExt; +use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + thread, + time::{Duration, Instant}, +}; +use ta::indicators::ExponentialMovingAverage; +use ta::Next; + +/// Maximum number of SubDevices that can be stored. This must be a power of 2 greater than 1. +const MAX_SUBDEVICES: usize = 16; +const MAX_PDU_DATA: usize = PduStorage::element_size(1100); +const MAX_FRAMES: usize = 32; +const PDI_LEN: usize = 64; + +static PDU_STORAGE: PduStorage = PduStorage::new(); + +const TICK_INTERVAL: Duration = Duration::from_millis(5); + +fn main() -> Result<(), Error> { + env_logger::Builder::from_env(Env::default().default_filter_or("info")).init(); + + let interface = std::env::args() + .nth(1) + .expect("Provide network interface as first argument."); + + log::info!("Starting DS402 demo..."); + log::info!("Run with RUST_LOG=ethercrab=debug or =trace for debug information"); + + let (tx, rx, pdu_loop) = PDU_STORAGE.try_split().expect("can only split once"); + + let maindevice = Arc::new(MainDevice::new( + pdu_loop, + Timeouts { + wait_loop_delay: Duration::from_millis(5), + state_transition: Duration::from_secs(10), + pdu: Duration::from_millis(2000), + ..Timeouts::default() + }, + MainDeviceConfig { + dc_static_sync_iterations: 1000, + ..MainDeviceConfig::default() + }, + )); + + let mut tick_interval = smol::Timer::interval(TICK_INTERVAL); + + #[cfg(target_os = "windows")] + std::thread::spawn(move || { + ethercrab::std::tx_rx_task_blocking( + &interface, + tx, + rx, + ethercrab::std::TxRxTaskConfig { spinloop: false }, + ) + .expect("TX/RX task") + }); + #[cfg(not(target_os = "windows"))] + smol::spawn(ethercrab::std::tx_rx_task(&interface, tx, rx).expect("spawn TX/RX task")).detach(); + + // Wait for TX/RX loop to start + thread::sleep(Duration::from_millis(200)); + + #[cfg(target_os = "linux")] + thread_priority::set_current_thread_priority(thread_priority::ThreadPriority::Crossplatform( + thread_priority::ThreadPriorityValue::try_from(48u8).unwrap(), + )) + .expect("Main thread prio"); + + smol::block_on(async { + let mut group = maindevice + .init_single_group::(ethercat_now) + .await + .expect("Init"); + + // The group will be in PRE-OP at this point + + for mut subdevice in group.iter_mut(&maindevice) { + if subdevice.name() == "PD4-EB59CD-E-65-1" { + log::info!("Found servo"); + + // Enable SYNC0 DC + subdevice.set_dc_sync(DcSync::Sync0); + } + } + + log::info!("Group has {} SubDevices", group.len()); + + let mut averages = Vec::new(); + + for _ in 0..group.len() { + averages.push(ExponentialMovingAverage::new(64).unwrap()); + } + + log::info!("Moving into PRE-OP with PDI"); + + let group = group.into_pre_op_pdi(&maindevice).await?; + + log::info!("Done. PDI available. Waiting for SubDevices to align"); + + let mut now = Instant::now(); + let start = Instant::now(); + + // Repeatedly send group PDI and sync frame to align all SubDevice clocks. We use an + // exponential moving average of each SubDevice's deviation from the EtherCAT System Time + // (the time in the DC reference SubDevice) and take the maximum deviation. When that is + // below 100us (arbitraily chosen value for this demo), we call the sync good enough and + // exit the loop. + loop { + group + .tx_rx_sync_system_time(&maindevice) + .await + .expect("TX/RX"); + + if now.elapsed() >= Duration::from_millis(25) { + now = Instant::now(); + + let mut max_deviation = 0; + + for (s1, ema) in group.iter(&maindevice).zip(averages.iter_mut()) { + let diff = match s1 + .register_read::(RegisterAddress::DcSystemTimeDifference) + .await + { + Ok(value) => + // The returned value is NOT in two's compliment, rather the upper bit specifies + // whether the number in the remaining bits is odd or even, so we convert the + // value to `i32` using that logic here. + { + let flag = 0b1u32 << 31; + + if value >= flag { + // Strip off negative flag bit and negate value as normal + -((value & !flag) as i32) + } else { + value as i32 + } + } + Err(Error::WorkingCounter { .. }) => 0, + Err(e) => return Err(e), + }; + + let ema_next = ema.next(diff as f64); + + max_deviation = max_deviation.max(ema_next.abs() as u32); + } + + log::debug!("--> Max deviation {} ns", max_deviation); + + // Less than 100us max deviation + if max_deviation < 100_000 { + log::info!("Clocks settled after {} ms", start.elapsed().as_millis()); + + break; + } + } + + tick_interval.next().await; + } + + log::info!("Alignment done"); + + // SubDevice clocks are aligned. We can turn DC on now. + let group = group + .configure_dc_sync( + &maindevice, + DcConfiguration { + // Start SYNC0 100ms in the future + start_delay: Duration::from_millis(100), + // SYNC0 period should be the same as the process data loop in most cases + sync0_period: TICK_INTERVAL, + // Send process data half way through cycle + sync0_shift: TICK_INTERVAL / 2, + }, + ) + .await?; + + let group = group + .into_safe_op(&maindevice) + .await + .expect("PRE-OP -> SAFE-OP"); + + log::info!("SAFE-OP"); + + // Request OP state without waiting for all SubDevices to reach it. Allows the immediate + // start of the process data cycle, which is required when DC sync is used, otherwise + // SubDevices never reach OP, most often timing out with a SyncManagerWatchdog error. + let group = group + .request_into_op(&maindevice) + .await + .expect("SAFE-OP -> OP"); + + log::info!("OP requested"); + + let op_request = Instant::now(); + + // Send PDI and check group state until all SubDevices enter OP state. At this point, we can + // exit this loop and enter the main process data loop that does not have the state check + // overhead present here. + loop { + let now = Instant::now(); + + let response @ TxRxResponse { + working_counter: _wkc, + extra: CycleInfo { + next_cycle_wait, .. + }, + .. + } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + + if response.all_op() { + break; + } + + smol::Timer::at(now + next_cycle_wait).await; + } + + log::info!( + "All SubDevices entered OP in {} us", + op_request.elapsed().as_micros() + ); + + let term = Arc::new(AtomicBool::new(false)); + signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term)) + .expect("Register hook"); + + // Main application process data cycle + loop { + let now = Instant::now(); + + let TxRxResponse { + working_counter: _wkc, + extra: CycleInfo { + next_cycle_wait, .. + }, + .. + } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + + smol::Timer::at(now + next_cycle_wait).await; + + if term.load(Ordering::Relaxed) { + log::info!("Exiting..."); + + break; + } + } + + let group = group + .into_safe_op(&maindevice) + .await + .expect("OP -> SAFE-OP"); + + log::info!("OP -> SAFE-OP"); + + let group = group + .into_pre_op(&maindevice) + .await + .expect("SAFE-OP -> PRE-OP"); + + log::info!("SAFE-OP -> PRE-OP"); + + let _group = group.into_init(&maindevice).await.expect("PRE-OP -> INIT"); + + log::info!("PRE-OP -> INIT, shutdown complete"); + + Ok(()) + }) +} From a7a07e2deeff29631ed8de615ea3d776484088a8 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 15:07:15 +0000 Subject: [PATCH 02/31] Add DS402 raw control and status words --- src/ds402.rs | 81 ++++++++++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 3 ++ 2 files changed, 84 insertions(+) create mode 100644 src/ds402.rs diff --git a/src/ds402.rs b/src/ds402.rs new file mode 100644 index 00000000..67dfe981 --- /dev/null +++ b/src/ds402.rs @@ -0,0 +1,81 @@ +use ethercrab_wire::EtherCrabWireRead; + +/// DS402 control word (object 0x6040). +/// +/// ETG6010 5.2. +#[derive(Debug, Copy, Clone, EtherCrabWireRead)] +#[wire(bytes = 2)] +pub struct ControlWord { + #[wire(bits = 1)] + switch_on: bool, + #[wire(bits = 1)] + enable_voltage: bool, + #[wire(bits = 1)] + quick_stop: bool, + #[wire(bits = 1)] + enable_operation: bool, + #[wire(bits = 1)] + op_specific_1: bool, + #[wire(bits = 1)] + op_specific_2: bool, + #[wire(bits = 1)] + op_specific_3: bool, + #[wire(bits = 1)] + fault_reset: bool, + #[wire(bits = 1)] + halt: bool, + #[wire(bits = 1)] + op_specific_4: bool, + #[wire(bits = 1)] + reserved: bool, + #[wire(bits = 1)] + manf_1: bool, + #[wire(bits = 1)] + manf_2: bool, + #[wire(bits = 1)] + manf_3: bool, + #[wire(bits = 1)] + manf_4: bool, + #[wire(bits = 1)] + manf_5: bool, +} + +/// DS402 status word (object 0x6041). +/// +/// ETG6010 5.3. +#[derive(Debug, Copy, Clone, EtherCrabWireRead)] +#[wire(bytes = 2)] +pub struct StatusWord { + #[wire(bits = 1)] + ready_to_switch_on: bool, + #[wire(bits = 1)] + switched_on: bool, + #[wire(bits = 1)] + operation_enabled: bool, + #[wire(bits = 1)] + fault: bool, + #[wire(bits = 1)] + voltage_enabled: bool, + #[wire(bits = 1)] + quick_stop: bool, + #[wire(bits = 1)] + switch_on_disabled: bool, + #[wire(bits = 1)] + warning: bool, + #[wire(bits = 1)] + manf_1: bool, + #[wire(bits = 1)] + remote: bool, + #[wire(bits = 1)] + op_specific_1: bool, + #[wire(bits = 1)] + internal_limit_active: bool, + #[wire(bits = 1)] + op_specific_2: bool, + #[wire(bits = 1)] + op_specific_3: bool, + #[wire(bits = 1)] + manf_2: bool, + #[wire(bits = 1)] + manf_3: bool, +} diff --git a/src/lib.rs b/src/lib.rs index 0880324f..7c05d987 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -164,6 +164,7 @@ mod coe; mod command; mod dc; mod dl_status; +mod ds402; mod eeprom; pub mod error; mod ethernet; @@ -185,9 +186,11 @@ mod vendors; #[cfg(feature = "std")] pub mod std; +// TODO: Constrain this pub use al_status_code::AlStatusCode; pub use coe::SubIndex; pub use command::{Command, Reads, WrappedRead, WrappedWrite, Writes}; +pub use ds402::*; pub use ethercrab_wire::{ EtherCrabWireRead, EtherCrabWireReadSized, EtherCrabWireReadWrite, EtherCrabWireSized, EtherCrabWireWrite, EtherCrabWireWriteSized, From 059d9ffa3ea84077a7813ce3cf0011a6637cbf15 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 15:16:14 +0000 Subject: [PATCH 03/31] Add operation mode enum --- src/ds402.rs | 32 +++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/src/ds402.rs b/src/ds402.rs index 67dfe981..b9c1dc77 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -1,4 +1,4 @@ -use ethercrab_wire::EtherCrabWireRead; +use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite}; /// DS402 control word (object 0x6040). /// @@ -79,3 +79,33 @@ pub struct StatusWord { #[wire(bits = 1)] manf_3: bool, } + +/// Operation mode (objects 0x6060, 0x6061, 0x6502). +#[derive(Debug, Copy, Clone, EtherCrabWireReadWrite)] +#[wire(bytes = 1)] +#[repr(i8)] +pub enum OpMode { + /// Profile position mode, "PP". + ProfilePosition = 1, + /// Velocity mode (frequency converter), "VL". + Velocity = 2, + /// Profile velocity mode, "PV". + ProfileVelocity = 3, + /// Torque profile mode, "TQ". + ProfileTorque = 4, + /// Homing mode, "HM". + Homing = 6, + /// Interpolated position mode, "IP". + InterpolatedPosition = 7, + /// Cyclic synchronous position mode, "CSP". + CyclicSynchronousPosition = 8, + /// Cyclic synchronous velocity mode, "CSV". + CyclicSynchronousVelocity = 9, + /// Cyclic synchronous torque mode, "CST". + CyclicSynchronousTorque = 10, + /// Cyclic synchronous torque mode with commutation angle, "CSTCA". + CyclicSynchronousTorqueWithCommutation = 11, + /// Manufacturer specific mode from `-128..=-1`. + #[wire(catch_all)] + ManufacturerSpecific(i8), +} From 5073ff6820f0121af0943711552b9347e1a4e89a Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 15:45:24 +0000 Subject: [PATCH 04/31] Get drive into OP with 1ms DC cycle --- examples/ds402.rs | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index 0e1982f5..03608560 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -30,7 +30,8 @@ const PDI_LEN: usize = 64; static PDU_STORAGE: PduStorage = PduStorage::new(); -const TICK_INTERVAL: Duration = Duration::from_millis(5); +// This must remain at 1ms to get the drive into OP. The ESI file specifies this value. +const TICK_INTERVAL: Duration = Duration::from_millis(1); fn main() -> Result<(), Error> { env_logger::Builder::from_env(Env::default().default_filter_or("info")).init(); @@ -94,6 +95,32 @@ fn main() -> Result<(), Error> { if subdevice.name() == "PD4-EB59CD-E-65-1" { log::info!("Found servo"); + subdevice + .sdo_write_array( + 0x1600, + [ + 0x6040_0010u32, // Control word, 16 bits + 0x6060_0008, // Op mode, 8 bits + ], + ) + .await?; + + subdevice + .sdo_write_array( + 0x1a00, + [ + 0x6041_0010u32, // Status word, 16 bits + 0x6061_0008, // Op mode reported + ], + ) + .await?; + + // Outputs to SubDevice + subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; + + // Inputs back to MainDevice + subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + // Enable SYNC0 DC subdevice.set_dc_sync(DcSync::Sync0); } @@ -184,8 +211,8 @@ fn main() -> Result<(), Error> { start_delay: Duration::from_millis(100), // SYNC0 period should be the same as the process data loop in most cases sync0_period: TICK_INTERVAL, - // Send process data half way through cycle - sync0_shift: TICK_INTERVAL / 2, + // Taken from ESI file + sync0_shift: Duration::from_nanos(250_000), }, ) .await?; From 2ed5be9253cab3d48e2c15f8bbf7b1a33f80e615 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 16:44:14 +0000 Subject: [PATCH 05/31] Loosen `outputs()` signature This is so we can use `inputs()` and `outputs()` simultaneously. --- src/subdevice/pdi.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/subdevice/pdi.rs b/src/subdevice/pdi.rs index bf103b59..9a3ebfbb 100644 --- a/src/subdevice/pdi.rs +++ b/src/subdevice/pdi.rs @@ -54,7 +54,9 @@ impl PdiIoRawWriteGuard<'_, N> { &all[self.ranges.input.bytes.clone()] } - pub fn outputs(&mut self) -> &mut [u8] { + // SAFETY: We return a mutable slice because a) it's locked and we currently hold the lock and + // b) it is assumed to be non-overlapping with the slice returned by `inputs()`. + pub fn outputs(&self) -> &mut [u8] { let all = unsafe { &mut *self.lock.get() }.as_mut_slice(); &mut all[self.ranges.output.bytes.clone()] From f8b5e34e234902c97a6a6df0d0e4e6ec3eb4b40d Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 16:44:58 +0000 Subject: [PATCH 06/31] Export `ds402` module itself Keeps things cleaner --- src/ds402.rs | 2 ++ src/lib.rs | 4 +--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ds402.rs b/src/ds402.rs index b9c1dc77..3beafe5d 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -1,3 +1,5 @@ +//! DS402/CiA402 high level interface. + use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite}; /// DS402 control word (object 0x6040). diff --git a/src/lib.rs b/src/lib.rs index 7c05d987..bb437dbf 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -164,7 +164,7 @@ mod coe; mod command; mod dc; mod dl_status; -mod ds402; +pub mod ds402; mod eeprom; pub mod error; mod ethernet; @@ -186,11 +186,9 @@ mod vendors; #[cfg(feature = "std")] pub mod std; -// TODO: Constrain this pub use al_status_code::AlStatusCode; pub use coe::SubIndex; pub use command::{Command, Reads, WrappedRead, WrappedWrite, Writes}; -pub use ds402::*; pub use ethercrab_wire::{ EtherCrabWireRead, EtherCrabWireReadSized, EtherCrabWireReadWrite, EtherCrabWireSized, EtherCrabWireWrite, EtherCrabWireWriteSized, From 0ca38bdaed3159616bf95d13b37d51532a4c6f40 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Feb 2025 16:45:16 +0000 Subject: [PATCH 07/31] Extremely baseic PDI I and O --- examples/ds402.rs | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index 03608560..d2b39356 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -5,10 +5,12 @@ use env_logger::Env; use ethercrab::{ + ds402::{OpMode, StatusWord}, error::Error, std::ethercat_now, subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, - DcSync, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, Timeouts, + DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, + RegisterAddress, Timeouts, }; use futures_lite::StreamExt; use std::{ @@ -266,6 +268,8 @@ fn main() -> Result<(), Error> { signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term)) .expect("Register hook"); + let mut sd = group.subdevice(&maindevice, 0)?; + // Main application process data cycle loop { let now = Instant::now(); @@ -278,6 +282,25 @@ fn main() -> Result<(), Error> { .. } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + let io = sd.io_raw_mut(); + + let i = io.inputs(); + + let status_word = &i[0..2]; + let reported_op_mode = &i[2..3]; + + let mut o = io.outputs(); + + let control_word = &mut o[0..2]; + let op_mode = &mut o[2..3]; + + OpMode::CyclicSynchronousPosition.pack_to_slice(op_mode)?; + + let status_word = StatusWord::unpack_from_slice(status_word)?; + let reported_op_mode = OpMode::unpack_from_slice(reported_op_mode)?; + + println!("Op mode {:?}", reported_op_mode); + smol::Timer::at(now + next_cycle_wait).await; if term.load(Ordering::Relaxed) { From ea7000784a293bfa5e3d70d41af42e6ff1a705de Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Feb 2025 09:55:17 +0000 Subject: [PATCH 08/31] --wip-- [skip ci] --- src/ds402.rs | 121 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) diff --git a/src/ds402.rs b/src/ds402.rs index 3beafe5d..b190f413 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -111,3 +111,124 @@ pub enum OpMode { #[wire(catch_all)] ManufacturerSpecific(i8), } + +/// An object sent from the MainDevice to the SubDevice (RxPdo). +#[derive(Debug, Copy, Clone)] +#[repr(u32)] +pub enum WriteObject { + /// Control word. + ControlWord = 0x6040_0010, + + /// Operation mode. + OpMode = 0x6060_0008, +} + +/// An object received by the MainDevice from the SubDevice (TxPdo). +#[derive(Debug, Copy, Clone)] +#[repr(u32)] +pub enum ReadObject { + /// Status word. + StatusWord = 0x6041_0010, + + /// Operation mode. + OpMode = 0x6061_0008, +} + +/// SDO config for a SubDevice's read (with [`ReadObject`]) or write (with [`WriteObject`]) PDOs. +pub struct SyncManagerAssignment<'a, O> { + // Sync manager, start from 0x1C10 to 0x1C2F. + // TODO: Add an API to get SD read/write sync man by index? + /// Sync manager, starting from `0x1c12` for sync manager 0. + pub index: u16, + + /// PDO mappings. + pub mappings: &'a [PdoMapping<'a, O>], +} + +/// PDO object to be mapped. +pub struct PdoMapping<'a, O> { + /// PDO index, e.g. `0x1600` or `0x1a00`. + pub index: u16, + + /// PDO objects to map into this PDO. + pub objects: &'a [O], +} + +#[cfg(test)] +mod tests { + + use heapless::FnvIndexMap; + + use crate::{SubDevicePdi, SubDeviceRef}; + + use super::*; + + #[test] + fn raw() { + let outputs = &[SyncManagerAssignment { + index: 0x1c12, + mappings: &[PdoMapping { + index: 0x1600, + objects: &[WriteObject::ControlWord, WriteObject::OpMode], + }], + }]; + + let mut position = 0; + + let it = outputs + .iter() + .flat_map(|sm| sm.mappings) + .flat_map(|mapping| mapping.objects) + .map(|mapping| { + let object = *mapping as u32; + + let size = (object & 0xffff) as usize; + + let range = position..(position + size); + + position += size; + + ((object >> 16) as u16, range) + }); + + // PDI is locked during this closure + ds402.update(|| {}); + // loop { + // group.tx_rx(); + // ds402.tick(); + // } + + struct Ds402<'group, const MAX_PDI: usize, const ON: usize> { + outputs: FnvIndexMap, ON>, + subdevice: SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, + } + + impl<'group, const MAX_PDI: usize, const ON: usize> Ds402<'group, MAX_PDI, ON> { + fn set_op_mode(&mut self, mode: OpMode) -> Result<(), ()> { + match self.outputs.get_mut(&(WriteObject::OpMode as u16)) { + Some(v) => { + v = mode; + + Ok(()) + } + None => Err(()), + } + } + } + + let sd = Ds402::<32> { + outputs: FnvIndexMap::from_iter(it), + }; + + for (object, pdi_range) in sd.outputs { + println!( + "Object {:#06x}, {} PDI bytes at {:?}", + object, + pdi_range.len(), + pdi_range + ); + } + + sd.set_op_mode(OpMode::CyclicSynchronousPosition); + } +} From 06450e749735a87e102541afa63d36adf0b886b5 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 2 Mar 2025 12:02:59 +0000 Subject: [PATCH 09/31] --wip-- [skip ci] --- src/ds402.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/ds402.rs b/src/ds402.rs index b190f413..598bdcd7 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -191,7 +191,11 @@ mod tests { ((object >> 16) as u16, range) }); - // PDI is locked during this closure + // PDI is locked during this closure. Multiple SDs can update and the TX/RX loop will have + // to wait until they're done, so we should always get consistent state I think? + // Let's try to make this method &self so thread sharing is easier + // TODO: How do we provide the DC system time here? It might be in another thread. Maybe a + // reference to an atomic in the main group struct? ds402.update(|| {}); // loop { // group.tx_rx(); From 69f3660e4ba35138c74db17fc414607ae679fd2b Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 2 Mar 2025 13:41:40 +0000 Subject: [PATCH 10/31] Start doing the state machine stuff --- src/ds402.rs | 195 ++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 161 insertions(+), 34 deletions(-) diff --git a/src/ds402.rs b/src/ds402.rs index 598bdcd7..cfb79a55 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -1,6 +1,9 @@ //! DS402/CiA402 high level interface. -use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite}; +use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite, EtherCrabWireSized}; +use heapless::FnvIndexMap; + +use crate::{fmt, SubDevicePdi, SubDeviceRef}; /// DS402 control word (object 0x6040). /// @@ -42,6 +45,30 @@ pub struct ControlWord { manf_5: bool, } +impl ControlWord { + /// Set the desired state. + pub fn set_state(&mut self, state: WriteState) { + // Only reset faults if explicitly requested + self.fault_reset = false; + + match state { + WriteState::ResetFault => self.fault_reset = true, + WriteState::SwitchOn => { + self.switch_on = true; + } + WriteState::EnableVoltage => { + self.switch_on = true; + self.enable_voltage = true; + } + WriteState::EnableOperation => { + self.switch_on = true; + self.enable_voltage = true; + self.enable_operation = true; + } + } + } +} + /// DS402 status word (object 0x6041). /// /// ETG6010 5.3. @@ -82,6 +109,27 @@ pub struct StatusWord { manf_3: bool, } +impl StatusWord { + /// Read various fields of the status word and return the state machine state. + pub fn state(&self) -> ReadState { + if self.fault { + ReadState::Fault + } else if self.quick_stop { + ReadState::QuickStop + } else if self.operation_enabled { + ReadState::OpEnabled + } else if self.switched_on { + ReadState::SwitchedOn + } else if self.ready_to_switch_on { + ReadState::ReadyToSwitchOn + } else if self.switch_on_disabled { + ReadState::SwitchOnDisabled + } else { + ReadState::NotReadyToSwitchOn + } + } +} + /// Operation mode (objects 0x6060, 0x6061, 0x6502). #[derive(Debug, Copy, Clone, EtherCrabWireReadWrite)] #[wire(bytes = 1)] @@ -112,6 +160,65 @@ pub enum OpMode { ManufacturerSpecific(i8), } +/// Set the DS402 state machine state. +/// +/// This enum is used to set certain bits in the [`ControlWord`]. +#[derive(Debug, Copy, Clone)] +pub enum WriteState { + /// Reset fault. + ResetFault, + /// Switch on. + SwitchOn, + /// Enable voltage. + EnableVoltage, + /// Enable operation. + EnableOperation, +} + +/// DS402 state machine state. +/// +/// This enum is created from the individual bits in or [`StatusWord`]. +/// +/// ETG6010 5.1 Figure 2: State Machine +#[derive(Debug, Copy, Clone)] +pub enum ReadState { + /// Not ready to switch on. + NotReadyToSwitchOn, + /// Switch on disabled. + SwitchOnDisabled, + /// Ready to switch on. + ReadyToSwitchOn, + /// Switched on. + SwitchedOn, + /// Operation enabled. + OpEnabled, + /// Quick stop active. + QuickStop, + /// The device is in a fault state. + Fault, +} + +/// State machine transition. +#[derive(Debug, Copy, Clone)] +pub enum Transition { + /// The device is in a steady state. + Steady(ReadState), + /// The device is transitioning to a new desired state. + Transitioning { + /// Desired state. + desired: WriteState, + /// Current state. + actual: ReadState, + }, + /// The device has finished transitioning to a new state. + Edge { + /// Previous state before the transition started. + previous: ReadState, + /// Current state. + current: ReadState, + }, +} + /// An object sent from the MainDevice to the SubDevice (RxPdo). #[derive(Debug, Copy, Clone)] #[repr(u32)] @@ -154,25 +261,73 @@ pub struct PdoMapping<'a, O> { pub objects: &'a [O], } -#[cfg(test)] -mod tests { +/// Wrap a group SubDevice in a higher level DS402 API +pub struct Ds402<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> { + outputs: FnvIndexMap, MAX_OUTPUT_OBJECTS>, + // TODO: Inputs map + subdevice: SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, +} - use heapless::FnvIndexMap; +impl<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> + Ds402<'group, MAX_PDI, MAX_OUTPUT_OBJECTS> +{ + /// Set DS402 operation mode (CSV, CSP, etc). + // TODO: This will be a mandatory field at some point, so this specifically doesn't need + // to return a `Result`. + pub fn set_op_mode(&mut self, mode: OpMode) -> Result<(), ()> { + match self.outputs.get_mut(&(WriteObject::OpMode as u16)) { + Some(v) => { + // v = mode; + todo!(); + + Ok(()) + } + None => Err(()), + } + } - use crate::{SubDevicePdi, SubDeviceRef}; + /// Get the DS402 status word. + pub fn status_word(&self) -> StatusWord { + // TODO: Dynamically(?) compute + let state_range = 0..StatusWord::PACKED_LEN; + fmt::unwrap_opt!(self + .subdevice + .inputs_raw() + .get(state_range) + .and_then(|bytes| StatusWord::unpack_from_slice(bytes).ok())) + } + + /// Get the current DS402 state machine state. + pub fn state(&self) -> ReadState { + self.status_word().state() + } +} + +#[cfg(test)] +mod tests { use super::*; + use heapless::FnvIndexMap; #[test] fn raw() { + // SM configuration. Order matters! + // TODO: Make some fields mandatory: op mode, op mode status, supported drive modes. This is + // required by ETG6010 Table 8: Modes of operation – Object list let outputs = &[SyncManagerAssignment { + // TODO: Higher level API so we can get the correct read/write SM address from the + // subdevice (e.g. `sd.read_sm(0) -> Option` or something) index: 0x1c12, + // TODO: Validate that the SM can have this many mappings mappings: &[PdoMapping { index: 0x1600, + // TODO: Validate that this mapping object can have this many PDOs, e.g. some SD + // PDOs can only have 4 assignments objects: &[WriteObject::ControlWord, WriteObject::OpMode], }], }]; + // PDI offset accumulator let mut position = 0; let it = outputs @@ -191,37 +346,9 @@ mod tests { ((object >> 16) as u16, range) }); - // PDI is locked during this closure. Multiple SDs can update and the TX/RX loop will have - // to wait until they're done, so we should always get consistent state I think? - // Let's try to make this method &self so thread sharing is easier - // TODO: How do we provide the DC system time here? It might be in another thread. Maybe a - // reference to an atomic in the main group struct? - ds402.update(|| {}); - // loop { - // group.tx_rx(); - // ds402.tick(); - // } - - struct Ds402<'group, const MAX_PDI: usize, const ON: usize> { - outputs: FnvIndexMap, ON>, - subdevice: SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, - } - - impl<'group, const MAX_PDI: usize, const ON: usize> Ds402<'group, MAX_PDI, ON> { - fn set_op_mode(&mut self, mode: OpMode) -> Result<(), ()> { - match self.outputs.get_mut(&(WriteObject::OpMode as u16)) { - Some(v) => { - v = mode; - - Ok(()) - } - None => Err(()), - } - } - } - let sd = Ds402::<32> { outputs: FnvIndexMap::from_iter(it), + subdevice: todo!(), }; for (object, pdi_range) in sd.outputs { From b096e9b55874803d9bd33e6cbd6ddd90e2186932 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sat, 15 Mar 2025 21:29:13 +0000 Subject: [PATCH 11/31] Thought through some ideas on SD config --- NOTES.md | 75 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) diff --git a/NOTES.md b/NOTES.md index eb33f607..482ed081 100644 --- a/NOTES.md +++ b/NOTES.md @@ -10,6 +10,81 @@ this register. SOEM calls `0x0990` `ECT_REG_DCSTART0` and also writes a 64 bit value into it. See `ethercatdc.c`, `ecx_dcsync0`. +# Overriding SM and FMMU config + +A key problem is wanting to store the SD config on `SubDevice` before it's used, meaning we'd either +have to put lifetimes or (const) generics everywhere. Seeing as the config is actually used during +SAFE-OP -> PRE-OP, why not do everything there with a callback: + +```rust +let mut servo_mapping = None; + +group.into_pre_op_with_config(|sd: SubDeviceRef<&SubDevice>, index_in_group: usize| { + if sd.name() == "EL3702" { + let config = /* ... */ + + servo_mapping = Some(config.pdi_mapping()); + + Ok(Some(config)) + } else if { + sd.sdo_write(0x1601, 0, 0x6040_0010).await?; + + // Let EtherCrab configure everything for me, most likely from CoE + Ok(None) + } else { + // Let EtherCrab configure everything for me, most likely from EEPROM + Ok(None) + } +}); + +let servo = servo_mapping.map(|m| Ds402 { pdi_mapping: m }).unwrap(); +``` + +We should also probably get rid of `into_pre_op_pdi` and just expose the PDI in whatever's returned +from `into_pre_op` - there's no point in splitting that behaviour. Let's just deprecate +`into_pre_op_pdi` for now and move all the behaviour into `into_pre_op`. + +## Old crap, unfinished thoughts, etc: + +Store a reference to some structure that defines SM mappings. This could be handwritten, or could +come from a parsed ESI file. This structure holds mappings for the PDOs for each SM. EtherCrab +figures out and computes the FMMU offsets internally, at least for now. Can always add more fields +to config structs. + +This needs to work for oversampling config (i.e. turning `u16` into `[u16; N]` or whatever). + +```rust +#[non_exhaustive] +pub struct SubDeviceMappingConfig { + mappings: &[/* ... */], + // Might add other flags here in the future, like "disable CoE write", "force sumn sumn" etc. +} +``` + +```rust +const fn pdo(index: u16, sub_index: u8) -> u32 { + let size_bits = T::packed_len * 8; + + todo!() +} +``` + +### Implementation + +> No: I don't want to put a lifetime on the `SubDevice`, so I can't store the slice of SM configs. + +Optionally set configuration on SD during init. This config should be used to completely configure +the SD's SMs and FMMUs automatically within EtherCrab during the next transition. If the SD supports +CoE, this config object can also be used to write all the CoE registers. Is this actually necessary +though? No I don't think it is - currently, EtherCrab writes to the CoE stuff then re-reads that +config when configuring via CoE (i.e. not EEPROM). I think we should then be able to completely +bypass all this stuff and just write the whole config into the SMs/FMMUs. + +Then, once the SD has a PDI, add a method to get a single linear `FnvIndexMap` from PDO object +(`0x6060`, etc) into the SD's PDI (`Range`) (not the global address, at least for now). + +All this stuff should be made in a way that helps with DS402, but should deal with raw numbers to +make it generically useful. # Configuring SDOs in a less error prone way From f5cfe1b57ba899f2d0a34303c6d7fa1e232ca076 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sat, 15 Mar 2025 21:29:36 +0000 Subject: [PATCH 12/31] A bunch of... stuff? --- examples/ds402.rs | 83 +++++++++++++++++++++----------- src/ds402.rs | 99 ++++++++++++++++++++++++-------------- src/generate.rs | 4 +- src/subdevice_group/mod.rs | 24 +++++++++ 4 files changed, 143 insertions(+), 67 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index d2b39356..c0ad1ed0 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -5,7 +5,7 @@ use env_logger::Env; use ethercrab::{ - ds402::{OpMode, StatusWord}, + ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, error::Error, std::ethercat_now, subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, @@ -93,35 +93,62 @@ fn main() -> Result<(), Error> { // The group will be in PRE-OP at this point + // let mut servo = None; + for mut subdevice in group.iter_mut(&maindevice) { if subdevice.name() == "PD4-EB59CD-E-65-1" { - log::info!("Found servo"); - - subdevice - .sdo_write_array( - 0x1600, - [ - 0x6040_0010u32, // Control word, 16 bits - 0x6060_0008, // Op mode, 8 bits - ], - ) - .await?; - - subdevice - .sdo_write_array( - 0x1a00, - [ - 0x6041_0010u32, // Status word, 16 bits - 0x6061_0008, // Op mode reported + log::info!("Found servo {:?}", subdevice.identity()); + + // subdevice + // .sdo_write_array( + // 0x1600, + // [ + // 0x6040_0010u32, // Control word, 16 bits + // 0x6060_0008, // Op mode, 8 bits + // ], + // ) + // .await?; + + // subdevice + // .sdo_write_array( + // 0x1a00, + // [ + // 0x6041_0010u32, // Status word, 16 bits + // 0x6061_0008, // Op mode reported + // ], + // ) + // .await?; + + // // Outputs to SubDevice + // subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; + + // // Inputs back to MainDevice + // subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + + let outputs_config = [SyncManagerAssignment { + // Outputs, 0x1c12 + sm: 2, + mappings: &[PdoMapping { + index: 0x1600, + objects: &[ + ds402::WriteObject::CONTROL_WORD, + ds402::WriteObject::OP_MODE, ], - ) - .await?; - - // Outputs to SubDevice - subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; - - // Inputs back to MainDevice - subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + // TODO: Oversampling config options + }], + }]; + + let inputs_config = [SyncManagerAssignment { + // Inputs, 0x1c13 + sm: 3, + mappings: &[PdoMapping { + index: 0x1a00, + objects: &[ds402::ReadObject::STATUS_WORD, ds402::ReadObject::OP_MODE], + }], + }]; + + // let (inputs_mapping, outputs_mapping) = + subdevice.set_config(&inputs_config, &outputs_config); // Enable SYNC0 DC subdevice.set_dc_sync(DcSync::Sync0); @@ -299,7 +326,7 @@ fn main() -> Result<(), Error> { let status_word = StatusWord::unpack_from_slice(status_word)?; let reported_op_mode = OpMode::unpack_from_slice(reported_op_mode)?; - println!("Op mode {:?}", reported_op_mode); + // println!("Op mode {:?}", reported_op_mode); smol::Timer::at(now + next_cycle_wait).await; diff --git a/src/ds402.rs b/src/ds402.rs index cfb79a55..5a1ae16b 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -220,45 +220,45 @@ pub enum Transition { } /// An object sent from the MainDevice to the SubDevice (RxPdo). -#[derive(Debug, Copy, Clone)] -#[repr(u32)] -pub enum WriteObject { +#[derive(Copy, Clone)] +pub struct WriteObject; + +impl WriteObject { /// Control word. - ControlWord = 0x6040_0010, + pub const CONTROL_WORD: u32 = 0x6040_0010; /// Operation mode. - OpMode = 0x6060_0008, + pub const OP_MODE: u32 = 0x6060_0008; } /// An object received by the MainDevice from the SubDevice (TxPdo). -#[derive(Debug, Copy, Clone)] -#[repr(u32)] -pub enum ReadObject { - /// Status word. - StatusWord = 0x6041_0010, +#[derive(Copy, Clone)] +pub struct ReadObject; + +impl ReadObject { + /// Control word. + pub const STATUS_WORD: u32 = 0x6041_0010; /// Operation mode. - OpMode = 0x6061_0008, + pub const OP_MODE: u32 = 0x6061_0008; } /// SDO config for a SubDevice's read (with [`ReadObject`]) or write (with [`WriteObject`]) PDOs. -pub struct SyncManagerAssignment<'a, O> { - // Sync manager, start from 0x1C10 to 0x1C2F. - // TODO: Add an API to get SD read/write sync man by index? - /// Sync manager, starting from `0x1c12` for sync manager 0. - pub index: u16, +pub struct SyncManagerAssignment<'a> { + /// Sync manager index starting from 0. + pub sm: u16, /// PDO mappings. - pub mappings: &'a [PdoMapping<'a, O>], + pub mappings: &'a [PdoMapping<'a>], } /// PDO object to be mapped. -pub struct PdoMapping<'a, O> { +pub struct PdoMapping<'a> { /// PDO index, e.g. `0x1600` or `0x1a00`. pub index: u16, /// PDO objects to map into this PDO. - pub objects: &'a [O], + pub objects: &'a [u32], } /// Wrap a group SubDevice in a higher level DS402 API @@ -275,7 +275,7 @@ impl<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> // TODO: This will be a mandatory field at some point, so this specifically doesn't need // to return a `Result`. pub fn set_op_mode(&mut self, mode: OpMode) -> Result<(), ()> { - match self.outputs.get_mut(&(WriteObject::OpMode as u16)) { + match self.outputs.get_mut(&((WriteObject::OP_MODE >> 16) as u16)) { Some(v) => { // v = mode; todo!(); @@ -306,6 +306,8 @@ impl<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> #[cfg(test)] mod tests { + use core::{mem::MaybeUninit, ops::Range}; + use super::*; use heapless::FnvIndexMap; @@ -317,41 +319,64 @@ mod tests { let outputs = &[SyncManagerAssignment { // TODO: Higher level API so we can get the correct read/write SM address from the // subdevice (e.g. `sd.read_sm(0) -> Option` or something) - index: 0x1c12, + // index: 0x1c12, + sm: 0, // TODO: Validate that the SM can have this many mappings mappings: &[PdoMapping { index: 0x1600, // TODO: Validate that this mapping object can have this many PDOs, e.g. some SD // PDOs can only have 4 assignments - objects: &[WriteObject::ControlWord, WriteObject::OpMode], + objects: &[WriteObject::CONTROL_WORD, WriteObject::OP_MODE], }], }]; // PDI offset accumulator let mut position = 0; - let it = outputs - .iter() - .flat_map(|sm| sm.mappings) - .flat_map(|mapping| mapping.objects) - .map(|mapping| { - let object = *mapping as u32; + /// Convert SM config into a list of offsets into the SubDevice's PDI. + fn config_to_offsets( + config: &[SyncManagerAssignment], + position_accumulator: &mut usize, + ) -> FnvIndexMap, N> { + let mut map = FnvIndexMap::new(); + + // Turn nice mapping object into linear list of ranges into the SD's PDI + config + .into_iter() + .flat_map(|sm| sm.mappings) + .flat_map(|mapping| mapping.objects) + .for_each(|mapping| { + let object = *mapping; + + let size_bytes = (object & 0xffff) as usize / 8; - let size = (object & 0xffff) as usize; + let range = *position_accumulator..(*position_accumulator + size_bytes); - let range = position..(position + size); + *position_accumulator += size_bytes; - position += size; + let key = (object >> 16) as u16; + + assert_eq!( + map.insert(key, range), + Ok(None), + "multiple mappings of object {:#06x}", + key + ); + }); + + map + } - ((object >> 16) as u16, range) - }); + let outputs = config_to_offsets(outputs, &mut position); - let sd = Ds402::<32> { - outputs: FnvIndexMap::from_iter(it), - subdevice: todo!(), + let mut sd = Ds402::<32, 8> { + outputs, + // subdevice: SubDeviceRef::new(maindevice, 0x1000, SubDevicePdi::default()), + // FIXME: This is HILARIOUSLY unsafe but I really cba to mock up all the right stuff atm + subdevice: unsafe { MaybeUninit::zeroed().assume_init() }, }; - for (object, pdi_range) in sd.outputs { + for (object, pdi_range) in sd.outputs.iter() { println!( "Object {:#06x}, {} PDI bytes at {:?}", object, diff --git a/src/generate.rs b/src/generate.rs index 80a857fa..582afb15 100644 --- a/src/generate.rs +++ b/src/generate.rs @@ -26,8 +26,8 @@ mod tests { let mut buf = [0u8, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]; assert_eq!(skip(10, &mut buf), &[10]); - assert_eq!(skip(11, &mut buf), &[]); - assert_eq!(skip(12, &mut buf), &[]); + // assert_eq!(skip(11, &mut buf), &[]); + // assert_eq!(skip(12, &mut buf), &[]); } #[test] diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 7cbca61f..6e3df8d6 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -276,6 +276,30 @@ impl )) } + /// Borrow an individual SubDevice, searched for by configured station address. + #[deny(clippy::panic)] + pub(crate) fn subdevice_by_configured_address<'maindevice, 'group>( + &'group self, + maindevice: &'maindevice MainDevice<'maindevice>, + configured_address: u16, + ) -> Result, Error> { + let subdevice = self + .inner() + .subdevices + .iter() + .find(|sd| sd.configured_address() == configured_address) + .ok_or(Error::NotFound { + item: Item::SubDevice, + index: Some(configured_address.into()), + })?; + + Ok(SubDeviceRef::new( + maindevice, + subdevice.configured_address(), + subdevice, + )) + } + /// Transition the group from PRE-OP -> SAFE-OP -> OP. /// /// To transition individually from PRE-OP to SAFE-OP, then SAFE-OP to OP, see From 0e17daa5f89faa981c01f83d46722a814fe10fd8 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Mar 2025 20:24:25 +0000 Subject: [PATCH 13/31] Extremely basic oversampling --- examples/ds402.rs | 172 ++++++++++----- examples/el3702-oversampling.rs | 380 ++++++++++++++++++++++++++++++++ src/ds402.rs | 40 +++- src/subdevice/configuration.rs | 47 +++- src/subdevice_group/mod.rs | 190 +++++++++++++--- 5 files changed, 738 insertions(+), 91 deletions(-) create mode 100644 examples/el3702-oversampling.rs diff --git a/examples/ds402.rs b/examples/ds402.rs index c0ad1ed0..0df3f0ca 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -95,65 +95,43 @@ fn main() -> Result<(), Error> { // let mut servo = None; - for mut subdevice in group.iter_mut(&maindevice) { - if subdevice.name() == "PD4-EB59CD-E-65-1" { - log::info!("Found servo {:?}", subdevice.identity()); - - // subdevice - // .sdo_write_array( - // 0x1600, - // [ - // 0x6040_0010u32, // Control word, 16 bits - // 0x6060_0008, // Op mode, 8 bits - // ], - // ) - // .await?; - - // subdevice - // .sdo_write_array( - // 0x1a00, - // [ - // 0x6041_0010u32, // Status word, 16 bits - // 0x6061_0008, // Op mode reported - // ], - // ) - // .await?; - - // // Outputs to SubDevice - // subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; - - // // Inputs back to MainDevice - // subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; - - let outputs_config = [SyncManagerAssignment { - // Outputs, 0x1c12 - sm: 2, - mappings: &[PdoMapping { - index: 0x1600, - objects: &[ - ds402::WriteObject::CONTROL_WORD, - ds402::WriteObject::OP_MODE, - ], - // TODO: Oversampling config options - }], - }]; - - let inputs_config = [SyncManagerAssignment { - // Inputs, 0x1c13 - sm: 3, - mappings: &[PdoMapping { - index: 0x1a00, - objects: &[ds402::ReadObject::STATUS_WORD, ds402::ReadObject::OP_MODE], - }], - }]; - - // let (inputs_mapping, outputs_mapping) = - subdevice.set_config(&inputs_config, &outputs_config); - - // Enable SYNC0 DC - subdevice.set_dc_sync(DcSync::Sync0); - } - } + // for mut subdevice in group.iter_mut(&maindevice) { + // if subdevice.name() == "PD4-EB59CD-E-65-1" { + // log::info!("Found servo {:?}", subdevice.identity()); + + // // subdevice + // // .sdo_write_array( + // // 0x1600, + // // [ + // // 0x6040_0010u32, // Control word, 16 bits + // // 0x6060_0008, // Op mode, 8 bits + // // ], + // // ) + // // .await?; + + // // subdevice + // // .sdo_write_array( + // // 0x1a00, + // // [ + // // 0x6041_0010u32, // Status word, 16 bits + // // 0x6061_0008, // Op mode reported + // // ], + // // ) + // // .await?; + + // // // Outputs to SubDevice + // // subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; + + // // // Inputs back to MainDevice + // // subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + + // // // let (inputs_mapping, outputs_mapping) = + // // subdevice.set_config(&inputs_config, &outputs_config); + + // // // Enable SYNC0 DC + // // subdevice.set_dc_sync(DcSync::Sync0); + // } + // } log::info!("Group has {} SubDevices", group.len()); @@ -165,7 +143,81 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); - let group = group.into_pre_op_pdi(&maindevice).await?; + let outputs_config = [SyncManagerAssignment { + // Outputs, 0x1c12 + sync_manager: 2, + fmmu: 0, + mappings: &[PdoMapping { + index: 0x1600, + objects: &[ + // TODO: Could these become enums again? + ds402::WriteObject::CONTROL_WORD, + ds402::WriteObject::OP_MODE, + ], + ..PdoMapping::default() + }], + }]; + + let inputs_config = [SyncManagerAssignment { + // Inputs, 0x1c13 + sync_manager: 3, + fmmu: 1, + mappings: &[PdoMapping { + index: 0x1a00, + objects: &[ds402::ReadObject::STATUS_WORD, ds402::ReadObject::OP_MODE], + ..PdoMapping::default() + }], + }]; + + let group = group + .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { + if subdevice.name() == "PD4-EB59CD-E-65-1" { + log::info!("Found servo {:?}", subdevice.identity()); + + // subdevice + // .sdo_write_array( + // 0x1600, + // [ + // 0x6040_0010u32, // Control word, 16 bits + // 0x6060_0008, // Op mode, 8 bits + // ], + // ) + // .await?; + + // subdevice + // .sdo_write_array( + // 0x1a00, + // [ + // 0x6041_0010u32, // Status word, 16 bits + // 0x6061_0008, // Op mode reported + // ], + // ) + // .await?; + + // // Outputs to SubDevice + // subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; + + // // Inputs back to MainDevice + // subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + + subdevice.set_dc_sync(DcSync::Sync0); + + Ok(Some((&inputs_config[..], &outputs_config[..]))) + // Ok(None) + } else { + Ok(None) + } + }) + .await?; + + for sd in group.iter(&maindevice) { + log::info!( + "--> {:#06x} PDI {} input bytes, {} output bytes", + sd.configured_address(), + sd.inputs_raw().len(), + sd.outputs_raw().len() + ); + } log::info!("Done. PDI available. Waiting for SubDevices to align"); diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs new file mode 100644 index 00000000..a6dab2e7 --- /dev/null +++ b/examples/el3702-oversampling.rs @@ -0,0 +1,380 @@ +//! Configure Distributed Clocks (DC) for EK1100 and a couple of other modules. +//! +//! Please note this example uses experimental features and should not be used as a reference for +//! other code. It is here (currently) primarily to help develop EtherCrab. + +use env_logger::Env; +use ethercrab::{ + ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, + error::Error, + std::ethercat_now, + subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, + DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, + RegisterAddress, Timeouts, +}; +use futures_lite::StreamExt; +use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + thread, + time::{Duration, Instant}, +}; +use ta::indicators::ExponentialMovingAverage; +use ta::Next; + +/// Maximum number of SubDevices that can be stored. This must be a power of 2 greater than 1. +const MAX_SUBDEVICES: usize = 16; +const MAX_PDU_DATA: usize = PduStorage::element_size(1100); +const MAX_FRAMES: usize = 32; +const PDI_LEN: usize = 64; + +static PDU_STORAGE: PduStorage = PduStorage::new(); + +// This must remain at 1ms to get the drive into OP. The ESI file specifies this value. +const TICK_INTERVAL: Duration = Duration::from_millis(1); + +fn main() -> Result<(), Error> { + env_logger::Builder::from_env(Env::default().default_filter_or("info")).init(); + + let interface = std::env::args() + .nth(1) + .expect("Provide network interface as first argument."); + + log::info!("Starting DS402 demo..."); + log::info!("Run with RUST_LOG=ethercrab=debug or =trace for debug information"); + + let (tx, rx, pdu_loop) = PDU_STORAGE.try_split().expect("can only split once"); + + let maindevice = Arc::new(MainDevice::new( + pdu_loop, + Timeouts { + wait_loop_delay: Duration::from_millis(5), + state_transition: Duration::from_secs(10), + pdu: Duration::from_millis(2000), + ..Timeouts::default() + }, + MainDeviceConfig { + dc_static_sync_iterations: 1000, + ..MainDeviceConfig::default() + }, + )); + + let mut tick_interval = smol::Timer::interval(TICK_INTERVAL); + + #[cfg(target_os = "windows")] + std::thread::spawn(move || { + ethercrab::std::tx_rx_task_blocking( + &interface, + tx, + rx, + ethercrab::std::TxRxTaskConfig { spinloop: false }, + ) + .expect("TX/RX task") + }); + #[cfg(not(target_os = "windows"))] + smol::spawn(ethercrab::std::tx_rx_task(&interface, tx, rx).expect("spawn TX/RX task")).detach(); + + // Wait for TX/RX loop to start + thread::sleep(Duration::from_millis(200)); + + #[cfg(target_os = "linux")] + thread_priority::set_current_thread_priority(thread_priority::ThreadPriority::Crossplatform( + thread_priority::ThreadPriorityValue::try_from(48u8).unwrap(), + )) + .expect("Main thread prio"); + + smol::block_on(async { + let mut group = maindevice + .init_single_group::(ethercat_now) + .await + .expect("Init"); + + // The group will be in PRE-OP at this point + + log::info!("Group has {} SubDevices", group.len()); + + let mut averages = Vec::new(); + + for _ in 0..group.len() { + averages.push(ExponentialMovingAverage::new(64).unwrap()); + } + + log::info!("Moving into PRE-OP with PDI"); + + let outputs_config = []; + + let inputs_config = [ + SyncManagerAssignment { + sync_manager: 0, + fmmu: 0, + mappings: &[ + // Ch1 cycle count + PdoMapping { + index: 0x1b00, + objects: &[0x6800_010f], + ..PdoMapping::default() + }, + // Ch1 first sample + PdoMapping { + index: 0x1a00, + objects: &[0x6000_010f], + oversampling: Some(2), + }, + ], + }, + SyncManagerAssignment { + sync_manager: 1, + fmmu: 1, + mappings: &[ + // Ch2 cycle count + PdoMapping { + index: 0x1b00, + // Sub index 2, 16 bits + objects: &[0x6800_020f], + ..PdoMapping::default() + }, + // Ch2 first sample + PdoMapping { + index: 0x1a80, + objects: &[0x6000_020f], + ..PdoMapping::default() + }, + ], + }, + ]; + + let group = group + .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { + if subdevice.name() == "EL3702" { + log::info!("Found EL3702 {:?}", subdevice.identity()); + + subdevice.set_dc_sync(DcSync::Sync01 { + sync1_period: Duration::from_millis(1), + }); + + Ok(Some((&inputs_config[..], &outputs_config[..]))) + // Ok(None) + } else { + Ok(None) + } + }) + .await?; + + for sd in group.iter(&maindevice) { + log::info!( + "--> {:#06x} PDI {} input bytes, {} output bytes", + sd.configured_address(), + sd.inputs_raw().len(), + sd.outputs_raw().len() + ); + } + + log::info!("Done. PDI available. Waiting for SubDevices to align"); + + let mut now = Instant::now(); + let start = Instant::now(); + + // Repeatedly send group PDI and sync frame to align all SubDevice clocks. We use an + // exponential moving average of each SubDevice's deviation from the EtherCAT System Time + // (the time in the DC reference SubDevice) and take the maximum deviation. When that is + // below 100us (arbitraily chosen value for this demo), we call the sync good enough and + // exit the loop. + loop { + group + .tx_rx_sync_system_time(&maindevice) + .await + .expect("TX/RX"); + + if now.elapsed() >= Duration::from_millis(25) { + now = Instant::now(); + + let mut max_deviation = 0; + + for (s1, ema) in group.iter(&maindevice).zip(averages.iter_mut()) { + let diff = match s1 + .register_read::(RegisterAddress::DcSystemTimeDifference) + .await + { + Ok(value) => + // The returned value is NOT in two's compliment, rather the upper bit specifies + // whether the number in the remaining bits is odd or even, so we convert the + // value to `i32` using that logic here. + { + let flag = 0b1u32 << 31; + + if value >= flag { + // Strip off negative flag bit and negate value as normal + -((value & !flag) as i32) + } else { + value as i32 + } + } + Err(Error::WorkingCounter { .. }) => 0, + Err(e) => return Err(e), + }; + + let ema_next = ema.next(diff as f64); + + max_deviation = max_deviation.max(ema_next.abs() as u32); + } + + log::debug!("--> Max deviation {} ns", max_deviation); + + // Less than 100us max deviation + if max_deviation < 100_000 { + log::info!("Clocks settled after {} ms", start.elapsed().as_millis()); + + break; + } + } + + tick_interval.next().await; + } + + log::info!("Alignment done"); + + // SubDevice clocks are aligned. We can turn DC on now. + let group = group + .configure_dc_sync( + &maindevice, + DcConfiguration { + // Start SYNC0 100ms in the future + start_delay: Duration::from_millis(100), + // SYNC0 period should be the same as the process data loop in most cases + sync0_period: TICK_INTERVAL, + // Taken from ESI file + sync0_shift: Duration::from_nanos(250_000), + }, + ) + .await?; + + let group = group + .into_safe_op(&maindevice) + .await + .expect("PRE-OP -> SAFE-OP"); + + log::info!("SAFE-OP"); + + // Request OP state without waiting for all SubDevices to reach it. Allows the immediate + // start of the process data cycle, which is required when DC sync is used, otherwise + // SubDevices never reach OP, most often timing out with a SyncManagerWatchdog error. + let group = group + .request_into_op(&maindevice) + .await + .expect("SAFE-OP -> OP"); + + log::info!("OP requested"); + + let op_request = Instant::now(); + + // Send PDI and check group state until all SubDevices enter OP state. At this point, we can + // exit this loop and enter the main process data loop that does not have the state check + // overhead present here. + loop { + let now = Instant::now(); + + let response @ TxRxResponse { + working_counter: _wkc, + extra: CycleInfo { + next_cycle_wait, .. + }, + .. + } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + + if response.all_op() { + break; + } + + smol::Timer::at(now + next_cycle_wait).await; + } + + log::info!( + "All SubDevices entered OP in {} us", + op_request.elapsed().as_micros() + ); + + let term = Arc::new(AtomicBool::new(false)); + signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term)) + .expect("Register hook"); + + let mut sd = group.subdevice(&maindevice, 0)?; + + // Main application process data cycle + loop { + let now = Instant::now(); + + let TxRxResponse { + working_counter: _wkc, + extra: CycleInfo { + next_cycle_wait, .. + }, + .. + } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + + for subdevice in group.iter(&maindevice) { + if subdevice.name().contains("EL4732") { + let mut o = subdevice.outputs_raw_mut(); + let voltage = -12.0_f64; + + // Convert voltage from -10V to +10V to a signed 16-bit value (-32768 to +32767) + let normalized = voltage / 10.0_f64; // Normalize to -1.0 to +1.0 range + let dac_value = (normalized * 32767.0_f64).round() as i16; + + // Extract bytes from the signed value + // For little-endian (low byte first) + o[2] = (dac_value & 0xFF) as u8; + o[3] = ((dac_value >> 8) & 0xFF) as u8; + + // For big-endian (high byte first) + // o[2] = ((dac_value >> 8) & 0xFF) as u8; + // o[3] = (dac_value & 0xFF) as u8; + } else if subdevice.name().contains("EL3702") { + let io = subdevice.io_raw(); + log::info!("EL3702 complete input data: {:02X?}", io.inputs()); + + let ch1_raw = i16::from_le_bytes([io.inputs()[2], io.inputs()[3]]); + let ch2_raw = i16::from_le_bytes([io.inputs()[6], io.inputs()[7]]); + + // Convert to voltage (+/-10V range) + let ch1_voltage = (ch1_raw as f32 / 32768.0) * 10.0; + let ch2_voltage = (ch2_raw as f32 / 32768.0) * 10.0; + + //log::info!( + // "EL3702 Inputs - CH1: {:+7.3} V (raw: {:+6}), CH2: {:+7.3} V (raw: {:+6})", + // ch1_voltage, ch1_raw, ch2_voltage, ch2_raw + //); + } + } + + smol::Timer::at(now + next_cycle_wait).await; + + if term.load(Ordering::Relaxed) { + log::info!("Exiting..."); + + break; + } + } + + let group = group + .into_safe_op(&maindevice) + .await + .expect("OP -> SAFE-OP"); + + log::info!("OP -> SAFE-OP"); + + let group = group + .into_pre_op(&maindevice) + .await + .expect("SAFE-OP -> PRE-OP"); + + log::info!("SAFE-OP -> PRE-OP"); + + let _group = group.into_init(&maindevice).await.expect("PRE-OP -> INIT"); + + log::info!("PRE-OP -> INIT, shutdown complete"); + + Ok(()) + }) +} diff --git a/src/ds402.rs b/src/ds402.rs index 5a1ae16b..0efeb243 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -244,21 +244,53 @@ impl ReadObject { } /// SDO config for a SubDevice's read (with [`ReadObject`]) or write (with [`WriteObject`]) PDOs. +#[derive(Debug, Copy, Clone)] pub struct SyncManagerAssignment<'a> { /// Sync manager index starting from 0. - pub sm: u16, + pub sync_manager: u8, + + /// Desired FMMU. + pub fmmu: u8, /// PDO mappings. pub mappings: &'a [PdoMapping<'a>], } +impl<'a> SyncManagerAssignment<'a> { + pub(crate) fn len_bits(&self) -> u16 { + self.mappings + .iter() + .map(|mapping| { + let mul = mapping.oversampling.unwrap_or(1).max(1); + + let sum: u16 = mapping + .objects + .iter() + .map(|object| { + // ETG1000.6 5.6.7.4.8; lower 8 bits are the object size + let size_bits = (object & 0xff) as u16; + + size_bits + }) + .sum(); + + sum * mul + }) + .sum() + } +} + /// PDO object to be mapped. +#[derive(Debug, Default, Copy, Clone)] pub struct PdoMapping<'a> { /// PDO index, e.g. `0x1600` or `0x1a00`. pub index: u16, /// PDO objects to map into this PDO. pub objects: &'a [u32], + + /// Oversampling ratio. If in doubt, set to `None``. + pub oversampling: Option, } /// Wrap a group SubDevice in a higher level DS402 API @@ -320,13 +352,17 @@ mod tests { // TODO: Higher level API so we can get the correct read/write SM address from the // subdevice (e.g. `sd.read_sm(0) -> Option` or something) // index: 0x1c12, - sm: 0, + sync_manager: 0, + + fmmu: 0, + // TODO: Validate that the SM can have this many mappings mappings: &[PdoMapping { index: 0x1600, // TODO: Validate that this mapping object can have this many PDOs, e.g. some SD // PDOs can only have 4 assignments objects: &[WriteObject::CONTROL_WORD, WriteObject::OP_MODE], + oversampling: None, }], }]; diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index dc2764d6..b01ee1c4 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -1,6 +1,7 @@ use super::{SubDevice, SubDeviceRef}; use crate::{ coe::{SdoExpedited, SubIndex}, + ds402::SyncManagerAssignment, eeprom::types::{ CoeDetails, DefaultMailbox, FmmuUsage, MailboxProtocols, SiiGeneral, SiiOwner, SyncManager, SyncManagerEnable, SyncManagerType, @@ -107,6 +108,7 @@ where mut global_offset: PdiOffset, group_start_address: u32, direction: PdoDirection, + config: Option<&[SyncManagerAssignment<'_>]>, ) -> Result { let eeprom = self.eeprom(); @@ -138,7 +140,10 @@ where has_coe ); - let range = if has_coe { + let range = if let Some(config) = config { + self.configure_pdos_config(&sync_managers, direction, &mut global_offset, config) + .await? + } else if has_coe { self.configure_pdos_coe(&sync_managers, &fmmu_usage, direction, &mut global_offset) .await? } else { @@ -592,6 +597,46 @@ where bytes: start_offset.up_to(*offset), }) } + + /// Configure PDOs from a given config. + async fn configure_pdos_config( + &self, + sync_managers: &[SyncManager], + direction: PdoDirection, + offset: &mut PdiOffset, + config: &[SyncManagerAssignment<'_>], + ) -> Result { + let start_offset = *offset; + + let (sm_type, _fmmu_type) = direction.filter_terms(); + + for assignment in config { + let sync_manager_index = assignment.sync_manager; + + let sync_manager = &sync_managers[usize::from(sync_manager_index)]; + + let fmmu_index = assignment.fmmu; + + let bit_len = assignment.len_bits(); + + let sm_config = self + .write_sm_config(sync_manager_index, sync_manager, (bit_len + 7) / 8) + .await?; + + self.write_fmmu_config( + bit_len, + usize::from(fmmu_index), + offset, + sm_type, + &sm_config, + ) + .await?; + } + + Ok(PdiSegment { + bytes: start_offset.up_to(*offset), + }) + } } #[derive(Copy, Clone)] diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 6e3df8d6..238953f5 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -8,15 +8,12 @@ mod handle; mod tx_rx_response; use crate::{ - DcSync, - MainDevice, - RegisterAddress, - SubDeviceState, + DcSync, MainDevice, RegisterAddress, SubDeviceState, al_control::AlControl, command::Command, + ds402::SyncManagerAssignment, error::{DistributedClockError, Error, Item}, fmt, - // lending_lock::LendingLock, pdi::PdiOffset, pdu_loop::{CreatedFrame, ReceivedPdu}, subdevice::{ @@ -189,6 +186,108 @@ pub struct SubDeviceGroup SubDeviceGroup { + async fn configure_fmmus2<'fun>( + &mut self, + maindevice: &MainDevice<'_>, + mut configure: impl AsyncFnMut( + SubDeviceRef<'_, &mut SubDevice>, + usize, + ) -> Result< + // TODO: Type alias two different typestates for inputs/outputs + Option<( + &'fun [SyncManagerAssignment<'fun>], + &'fun [SyncManagerAssignment<'fun>], + )>, + Error, + >, + ) -> Result<(), Error> { + let inner = self.inner.get_mut(); + + let mut pdi_position = inner.pdi_start; + + let mut configs = heapless::Vec::<_, MAX_SUBDEVICES>::new(); + + for (i, subdevice) in inner.subdevices.iter_mut().enumerate() { + // SAFETY: `configs` uses `MAX_SUBDEVICES` so should be the same length as or shorter + // than `inner.subdevices`. + fmt::unwrap!( + configs.push( + configure( + SubDeviceRef::new(maindevice, subdevice.configured_address(), subdevice), + i, + ) + .await?, + ) + ); + } + + fmt::debug!( + "Going to configure group with {} SubDevice(s), starting PDI offset {:#010x}", + inner.subdevices.len(), + inner.pdi_start.start_address + ); + + // Configure master read PDI mappings in the first section of the PDI + for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { + let inputs_config = config.map(|c| c.0); + + // We're in PRE-OP at this point + pdi_position = SubDeviceRef::new(maindevice, subdevice.configured_address(), subdevice) + .configure_fmmus( + pdi_position, + inner.pdi_start.start_address, + PdoDirection::MasterRead, + inputs_config, + ) + .await?; + } + + self.read_pdi_len = (pdi_position.start_address - inner.pdi_start.start_address) as usize; + + fmt::debug!("SubDevice mailboxes configured and init hooks called"); + + // We configured all read PDI mappings as a contiguous block in the previous loop. Now we'll + // configure the write mappings in a separate loop. This means we have IIIIOOOO instead of + // IOIOIO. + for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { + let outputs_config = config.map(|c| c.1); + + let addr = subdevice.configured_address(); + + let mut subdevice_config = SubDeviceRef::new(maindevice, addr, subdevice); + + // Still in PRE-OP + pdi_position = subdevice_config + .configure_fmmus( + pdi_position, + inner.pdi_start.start_address, + PdoDirection::MasterWrite, + outputs_config, + ) + .await?; + } + + fmt::debug!("SubDevice FMMUs configured for group. Able to move to SAFE-OP"); + + self.pdi_len = (pdi_position.start_address - inner.pdi_start.start_address) as usize; + + fmt::debug!( + "Group PDI length: start {:#010x}, {} total bytes ({} input bytes)", + inner.pdi_start.start_address, + self.pdi_len, + self.read_pdi_len + ); + + if self.pdi_len > MAX_PDI { + return Err(Error::PdiTooLong { + max_length: MAX_PDI, + desired_length: self.pdi_len, + }); + } + + Ok(()) + } + /// Configure read/write FMMUs and PDI for this group. async fn configure_fmmus(&mut self, maindevice: &MainDevice<'_>) -> Result<(), Error> { let inner = self.inner.get_mut(); @@ -209,6 +308,7 @@ impl pdi_position, inner.pdi_start.start_address, PdoDirection::MasterRead, + None, ) .await?; } @@ -231,6 +331,7 @@ impl pdi_position, inner.pdi_start.start_address, PdoDirection::MasterWrite, + None, ) .await?; } @@ -276,29 +377,29 @@ impl )) } - /// Borrow an individual SubDevice, searched for by configured station address. - #[deny(clippy::panic)] - pub(crate) fn subdevice_by_configured_address<'maindevice, 'group>( - &'group self, - maindevice: &'maindevice MainDevice<'maindevice>, - configured_address: u16, - ) -> Result, Error> { - let subdevice = self - .inner() - .subdevices - .iter() - .find(|sd| sd.configured_address() == configured_address) - .ok_or(Error::NotFound { - item: Item::SubDevice, - index: Some(configured_address.into()), - })?; - - Ok(SubDeviceRef::new( - maindevice, - subdevice.configured_address(), - subdevice, - )) - } + // /// Borrow an individual SubDevice, searched for by configured station address. + // #[deny(clippy::panic)] + // pub(crate) fn subdevice_by_configured_address<'maindevice, 'group>( + // &'group self, + // maindevice: &'maindevice MainDevice<'maindevice>, + // configured_address: u16, + // ) -> Result, Error> { + // let subdevice = self + // .inner() + // .subdevices + // .iter() + // .find(|sd| sd.configured_address() == configured_address) + // .ok_or(Error::NotFound { + // item: Item::SubDevice, + // index: Some(configured_address.into()), + // })?; + + // Ok(SubDeviceRef::new( + // maindevice, + // subdevice.configured_address(), + // subdevice, + // )) + // } /// Transition the group from PRE-OP -> SAFE-OP -> OP. /// @@ -313,6 +414,39 @@ impl self_.into_op(maindevice).await } + // NOTE: This just goes PRE-OP -> PRE-OP + PDI. What about into op and into safe op? + /// TODO: Docs + pub async fn into_pre_op_pdi_with_config<'fun>( + mut self, + maindevice: &MainDevice<'_>, + configure: impl AsyncFnMut( + SubDeviceRef<'_, &mut SubDevice>, + usize, + ) -> Result< + // TODO: Return a struct with `inputs` and `outputs` fields + Option<( + &'fun [SyncManagerAssignment<'fun>], + &'fun [SyncManagerAssignment<'fun>], + )>, + Error, + >, + ) -> Result, Error> + where + DC: 'fun, + { + self.configure_fmmus2(maindevice, configure).await?; + + Ok(SubDeviceGroup { + id: self.id, + pdi: self.pdi, + read_pdi_len: self.read_pdi_len, + pdi_len: self.pdi_len, + inner: self.inner, + dc_conf: self.dc_conf, + _state: PhantomData, + }) + } + /// Configure FMMUs, but leave the group in [`PreOp`] state. /// /// This method is used to obtain access to the group's PDI and related functionality. All SDO From bfb9223394aaa4f092084ad6c943221c208234d0 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 16 Mar 2025 22:27:45 +0000 Subject: [PATCH 14/31] Clean up mapping config creation with some const fns --- examples/el3702-oversampling.rs | 32 ++++++++----------------- src/ds402.rs | 41 +++++++++++++++++++++++++++++++++ src/lib.rs | 1 + 3 files changed, 52 insertions(+), 22 deletions(-) diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs index a6dab2e7..bf00532f 100644 --- a/examples/el3702-oversampling.rs +++ b/examples/el3702-oversampling.rs @@ -105,23 +105,20 @@ fn main() -> Result<(), Error> { let outputs_config = []; + const fn ass(index: u16, subindex: u8) -> u32 { + (index as u32) << 16 | (subindex as u32) << 8 | (2 as u32 * 8 & 0xff) + } + let inputs_config = [ SyncManagerAssignment { sync_manager: 0, fmmu: 0, mappings: &[ // Ch1 cycle count - PdoMapping { - index: 0x1b00, - objects: &[0x6800_010f], - ..PdoMapping::default() - }, + PdoMapping::new(0x1b00, const { &[PdoMapping::object::(0x6800, 1)] }), // Ch1 first sample - PdoMapping { - index: 0x1a00, - objects: &[0x6000_010f], - oversampling: Some(2), - }, + PdoMapping::new(0x1a00, const { &[PdoMapping::object::(0x6000, 1)] }) + .with_oversampling(2), ], }, SyncManagerAssignment { @@ -129,18 +126,10 @@ fn main() -> Result<(), Error> { fmmu: 1, mappings: &[ // Ch2 cycle count - PdoMapping { - index: 0x1b00, - // Sub index 2, 16 bits - objects: &[0x6800_020f], - ..PdoMapping::default() - }, + PdoMapping::new(0x1b00, const { &[PdoMapping::object::(0x6800, 2)] }), // Ch2 first sample - PdoMapping { - index: 0x1a80, - objects: &[0x6000_020f], - ..PdoMapping::default() - }, + PdoMapping::new(0x1a00, const { &[PdoMapping::object::(0x6000, 2)] }) + .with_oversampling(2), ], }, ]; @@ -155,7 +144,6 @@ fn main() -> Result<(), Error> { }); Ok(Some((&inputs_config[..], &outputs_config[..]))) - // Ok(None) } else { Ok(None) } diff --git a/src/ds402.rs b/src/ds402.rs index 0efeb243..efe06a47 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -293,6 +293,47 @@ pub struct PdoMapping<'a> { pub oversampling: Option, } +impl<'a> PdoMapping<'a> { + /// Create a new PDO assignment with the given index and PDO objects. + pub const fn new(index: u16, objects: &'a [u32]) -> Self { + Self { + index, + objects, + oversampling: None, + } + } + + /// Set oversampling configuration for this mapping. + /// + /// For actual oversampling, values over 1 should be used. + pub const fn with_oversampling(self, oversampling: u16) -> Self { + Self { + oversampling: Some(oversampling), + ..self + } + } + + /// Create an object mapping from an index, subindex and desired type. + /// + /// # Examples + /// + /// Map a 16 bit value at object `0x6800:01`. + /// + /// ```rust + /// use ethercrab::PdoMapping; + /// + /// let raw = PdoMapping::object::(0x6800, 1); + /// + /// assert_eq!(raw, 0x6800_010f); + /// ``` + pub const fn object(index: u16, subindex: u8) -> u32 + where + T: EtherCrabWireSized, + { + (index as u32) << 16 | (subindex as u32) << 8 | (T::PACKED_LEN as u32 * 8 & 0xff) + } +} + /// Wrap a group SubDevice in a higher level DS402 API pub struct Ds402<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> { outputs: FnvIndexMap, MAX_OUTPUT_OBJECTS>, diff --git a/src/lib.rs b/src/lib.rs index bb437dbf..d383d0ef 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -189,6 +189,7 @@ pub mod std; pub use al_status_code::AlStatusCode; pub use coe::SubIndex; pub use command::{Command, Reads, WrappedRead, WrappedWrite, Writes}; +pub use ds402::PdoMapping; pub use ethercrab_wire::{ EtherCrabWireRead, EtherCrabWireReadSized, EtherCrabWireReadWrite, EtherCrabWireSized, EtherCrabWireWrite, EtherCrabWireWriteSized, From ad235a6152220c8c1ebbbea29aa6a828d807ae11 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:17:40 +0000 Subject: [PATCH 15/31] Fix doc wording --- src/eeprom/types.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/eeprom/types.rs b/src/eeprom/types.rs index 95c4bd63..1236fdb9 100644 --- a/src/eeprom/types.rs +++ b/src/eeprom/types.rs @@ -557,9 +557,9 @@ pub enum SyncManagerType { MailboxWrite = 0x01, /// Used for reading from the SubDevice. MailboxRead = 0x02, - /// Used for process data outputs from MainDevice. + /// Used for process data outputs from MainDevice to SubDevice. ProcessDataWrite = 0x03, - /// Used for process data inputs to MainDevice. + /// Used for process data inputs into MainDevice from SubDevice. ProcessDataRead = 0x04, } From 224632e87677b75599de92f140243831415b1863 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:21:43 +0000 Subject: [PATCH 16/31] Make some mapping fields private --- examples/el3702-oversampling.rs | 77 +++++++++++++++++++-------------- src/ds402.rs | 44 ++++++++++++++++--- src/subdevice/configuration.rs | 40 ++++++++++++++++- src/subdevice_group/mod.rs | 32 ++++++-------- 4 files changed, 136 insertions(+), 57 deletions(-) diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs index bf00532f..12de83b1 100644 --- a/examples/el3702-oversampling.rs +++ b/examples/el3702-oversampling.rs @@ -8,7 +8,7 @@ use ethercrab::{ ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, error::Error, std::ethercat_now, - subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, + subdevice_group::{CycleInfo, DcConfiguration, MappingConfig, TxRxResponse}, DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, Timeouts, }; @@ -103,36 +103,49 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); - let outputs_config = []; - - const fn ass(index: u16, subindex: u8) -> u32 { - (index as u32) << 16 | (subindex as u32) << 8 | (2 as u32 * 8 & 0xff) - } - - let inputs_config = [ - SyncManagerAssignment { - sync_manager: 0, - fmmu: 0, - mappings: &[ - // Ch1 cycle count - PdoMapping::new(0x1b00, const { &[PdoMapping::object::(0x6800, 1)] }), - // Ch1 first sample - PdoMapping::new(0x1a00, const { &[PdoMapping::object::(0x6000, 1)] }) - .with_oversampling(2), - ], - }, - SyncManagerAssignment { - sync_manager: 1, - fmmu: 1, - mappings: &[ - // Ch2 cycle count - PdoMapping::new(0x1b00, const { &[PdoMapping::object::(0x6800, 2)] }), - // Ch2 first sample - PdoMapping::new(0x1a00, const { &[PdoMapping::object::(0x6000, 2)] }) - .with_oversampling(2), - ], - }, - ]; + let el3702_mapping = MappingConfig { + inputs: &[ + SyncManagerAssignment::new( + const { + &[ + // Ch1 cycle count + PdoMapping::new( + 0x1b00, + const { &[PdoMapping::object::(0x6800, 1)] }, + ), + // Ch1 first sample + PdoMapping::new( + 0x1a00, + const { &[PdoMapping::object::(0x6000, 1)] }, + ) + .with_oversampling(2), + ] + }, + ) + .with_sync_manager(0) + .with_fmmu(0), + SyncManagerAssignment::new( + const { + &[ + // Ch1 cycle count + PdoMapping::new( + 0x1b00, + const { &[PdoMapping::object::(0x6800, 2)] }, + ), + // Ch1 first sample + PdoMapping::new( + 0x1a00, + const { &[PdoMapping::object::(0x6000, 2)] }, + ) + .with_oversampling(2), + ] + }, + ) + .with_sync_manager(1) + .with_fmmu(1), + ][..], + outputs: &[], + }; let group = group .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { @@ -143,7 +156,7 @@ fn main() -> Result<(), Error> { sync1_period: Duration::from_millis(1), }); - Ok(Some((&inputs_config[..], &outputs_config[..]))) + Ok(Some(el3702_mapping)) } else { Ok(None) } diff --git a/src/ds402.rs b/src/ds402.rs index efe06a47..a0229675 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -2,6 +2,7 @@ use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite, EtherCrabWireSized}; use heapless::FnvIndexMap; +use libc::sync; use crate::{fmt, SubDevicePdi, SubDeviceRef}; @@ -223,6 +224,7 @@ pub enum Transition { #[derive(Copy, Clone)] pub struct WriteObject; +// TODO: Can this go back to being an enum? impl WriteObject { /// Control word. pub const CONTROL_WORD: u32 = 0x6040_0010; @@ -247,16 +249,48 @@ impl ReadObject { #[derive(Debug, Copy, Clone)] pub struct SyncManagerAssignment<'a> { /// Sync manager index starting from 0. - pub sync_manager: u8, + pub(crate) sync_manager: Option, /// Desired FMMU. - pub fmmu: u8, + pub(crate) fmmu: Option, /// PDO mappings. - pub mappings: &'a [PdoMapping<'a>], + pub(crate) mappings: &'a [PdoMapping<'a>], } impl<'a> SyncManagerAssignment<'a> { + /// Create a new SM assignment. + /// + /// The SubDevice Sync Manager and FMMU will be chosen automatically by EtherCrab, based on the + /// SubDevice's EEPROM contents. To override this behaviour, use + /// [`with_sync_manager`](SyncManagerAssignment::with_sync_manager) and/or + /// [`with_fmmu`](SyncManagerAssignment::with_fmmu). + pub fn new(mappings: &'a [PdoMapping<'a>]) -> Self { + Self { + mappings, + sync_manager: None, + fmmu: None, + } + } + + /// Set an explicit sync manager index to use. + pub fn with_sync_manager(self, sync_manager: u8) -> Self { + Self { + sync_manager: Some(sync_manager), + ..self + } + } + + /// Set an explicit FMMU index to use. + /// + /// This will be the same as the sync manager index most of the time. + pub fn with_fmmu(self, fmmu: u8) -> Self { + Self { + fmmu: Some(fmmu), + ..self + } + } + pub(crate) fn len_bits(&self) -> u16 { self.mappings .iter() @@ -393,9 +427,9 @@ mod tests { // TODO: Higher level API so we can get the correct read/write SM address from the // subdevice (e.g. `sd.read_sm(0) -> Option` or something) // index: 0x1c12, - sync_manager: 0, + sync_manager: None, - fmmu: 0, + fmmu: None, // TODO: Validate that the SM can have this many mappings mappings: &[PdoMapping { diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index b01ee1c4..6609e241 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -608,14 +608,50 @@ where ) -> Result { let start_offset = *offset; + let fmmu_sm_mappings = self.eeprom().fmmu_mappings().await?; + let (sm_type, _fmmu_type) = direction.filter_terms(); for assignment in config { - let sync_manager_index = assignment.sync_manager; + let sync_manager_index = assignment + .sync_manager + .or_else(|| { + // If SM is not explicitly set, find the first SM of the type we're looking for + let (index, kind) = sync_managers + .iter() + .enumerate() + .find(|(_idx, sm)| sm.usage_type == sm_type)?; + + Some(index as u8) + }) + .ok_or_else(|| { + fmt::error!( + "Failed to find sync manager {:?} for {:?}", + sm_type, + assignment + ); + + Error::Internal + })?; let sync_manager = &sync_managers[usize::from(sync_manager_index)]; - let fmmu_index = assignment.fmmu; + let fmmu_index = assignment.fmmu.unwrap_or_else(|| { + // If no FMMU was explicitly set, look for FMMU index using FMMU_EX section in + // EEPROM. If that fails, fall back to using the sync manager index instead. + fmmu_sm_mappings + .iter() + .find(|fmmu| fmmu.sync_manager == sync_manager_index) + .map(|fmmu| fmmu.sync_manager) + .unwrap_or_else(|| { + fmt::trace!( + "Could not find FMMU for PDO SM{} in EEPROM, using SM index to pick FMMU instead", + sync_manager_index, + ); + + sync_manager_index + }) + }); let bit_len = assignment.len_bits(); diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 238953f5..f63507bf 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -67,6 +67,16 @@ impl MySyncUnsafeCell { } } +/// TODO: Doc +#[derive(Debug, Copy, Clone)] +pub struct MappingConfig<'a> { + /// Input mappings (SubDevice -> MainDevice). + pub inputs: &'a [SyncManagerAssignment<'a>], + + /// Output mappings (MainDevice -> SubDevice). + pub outputs: &'a [SyncManagerAssignment<'a>], +} + /// A typestate for [`SubDeviceGroup`] representing a group that is shut down. /// /// This corresponds to the EtherCAT states INIT. @@ -192,14 +202,7 @@ impl mut configure: impl AsyncFnMut( SubDeviceRef<'_, &mut SubDevice>, usize, - ) -> Result< - // TODO: Type alias two different typestates for inputs/outputs - Option<( - &'fun [SyncManagerAssignment<'fun>], - &'fun [SyncManagerAssignment<'fun>], - )>, - Error, - >, + ) -> Result>, Error>, ) -> Result<(), Error> { let inner = self.inner.get_mut(); @@ -229,7 +232,7 @@ impl // Configure master read PDI mappings in the first section of the PDI for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { - let inputs_config = config.map(|c| c.0); + let inputs_config = config.map(|c| c.inputs); // We're in PRE-OP at this point pdi_position = SubDeviceRef::new(maindevice, subdevice.configured_address(), subdevice) @@ -250,7 +253,7 @@ impl // configure the write mappings in a separate loop. This means we have IIIIOOOO instead of // IOIOIO. for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { - let outputs_config = config.map(|c| c.1); + let outputs_config = config.map(|c| c.outputs); let addr = subdevice.configured_address(); @@ -422,14 +425,7 @@ impl configure: impl AsyncFnMut( SubDeviceRef<'_, &mut SubDevice>, usize, - ) -> Result< - // TODO: Return a struct with `inputs` and `outputs` fields - Option<( - &'fun [SyncManagerAssignment<'fun>], - &'fun [SyncManagerAssignment<'fun>], - )>, - Error, - >, + ) -> Result>, Error>, ) -> Result, Error> where DC: 'fun, From db0ae314050d289b92a180837691b6d12d80887d Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:26:26 +0000 Subject: [PATCH 17/31] Better SM finding code --- src/subdevice/configuration.rs | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index 6609e241..3568f814 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -613,16 +613,17 @@ where let (sm_type, _fmmu_type) = direction.filter_terms(); for assignment in config { - let sync_manager_index = assignment + let (sync_manager_index, sync_manager) = assignment .sync_manager + .and_then(|idx| sync_managers.get(usize::from(idx)).map(|sm| (idx, sm))) .or_else(|| { // If SM is not explicitly set, find the first SM of the type we're looking for - let (index, kind) = sync_managers + let (idx, sm) = sync_managers .iter() .enumerate() .find(|(_idx, sm)| sm.usage_type == sm_type)?; - Some(index as u8) + sync_managers.get(idx).map(|sm| (idx as u8, sm)) }) .ok_or_else(|| { fmt::error!( @@ -634,11 +635,20 @@ where Error::Internal })?; - let sync_manager = &sync_managers[usize::from(sync_manager_index)]; + if sync_manager.usage_type != sm_type { + fmt::error!( + "Sync manager index {} type is incorrect: {:?}, expected {:?}", + sync_manager_index, + sync_manager.usage_type, + sm_type + ); + + return Err(Error::Internal); + } let fmmu_index = assignment.fmmu.unwrap_or_else(|| { // If no FMMU was explicitly set, look for FMMU index using FMMU_EX section in - // EEPROM. If that fails, fall back to using the sync manager index instead. + // EEPROM. If that fails, fall back to using the sync manager index as FMMU index. fmmu_sm_mappings .iter() .find(|fmmu| fmmu.sync_manager == sync_manager_index) From 146285aa944d7ca6ff33d867ce07cc246bace557 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:26:48 +0000 Subject: [PATCH 18/31] Make builder fns const --- src/ds402.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ds402.rs b/src/ds402.rs index a0229675..6b854a3a 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -265,7 +265,7 @@ impl<'a> SyncManagerAssignment<'a> { /// SubDevice's EEPROM contents. To override this behaviour, use /// [`with_sync_manager`](SyncManagerAssignment::with_sync_manager) and/or /// [`with_fmmu`](SyncManagerAssignment::with_fmmu). - pub fn new(mappings: &'a [PdoMapping<'a>]) -> Self { + pub const fn new(mappings: &'a [PdoMapping<'a>]) -> Self { Self { mappings, sync_manager: None, @@ -274,7 +274,7 @@ impl<'a> SyncManagerAssignment<'a> { } /// Set an explicit sync manager index to use. - pub fn with_sync_manager(self, sync_manager: u8) -> Self { + pub const fn with_sync_manager(self, sync_manager: u8) -> Self { Self { sync_manager: Some(sync_manager), ..self @@ -284,7 +284,7 @@ impl<'a> SyncManagerAssignment<'a> { /// Set an explicit FMMU index to use. /// /// This will be the same as the sync manager index most of the time. - pub fn with_fmmu(self, fmmu: u8) -> Self { + pub const fn with_fmmu(self, fmmu: u8) -> Self { Self { fmmu: Some(fmmu), ..self From b8fa1524ae115a20f9b9c9636507a1c6127542e0 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:43:04 +0000 Subject: [PATCH 19/31] Make final fields private --- examples/el3702-oversampling.rs | 7 +++---- src/subdevice_group/mod.rs | 30 ++++++++++++++++++++++++++++-- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs index 12de83b1..0c29d30f 100644 --- a/examples/el3702-oversampling.rs +++ b/examples/el3702-oversampling.rs @@ -103,8 +103,8 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); - let el3702_mapping = MappingConfig { - inputs: &[ + let el3702_mapping = MappingConfig::inputs( + &[ SyncManagerAssignment::new( const { &[ @@ -144,8 +144,7 @@ fn main() -> Result<(), Error> { .with_sync_manager(1) .with_fmmu(1), ][..], - outputs: &[], - }; + ); let group = group .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index f63507bf..077f6180 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -71,10 +71,36 @@ impl MySyncUnsafeCell { #[derive(Debug, Copy, Clone)] pub struct MappingConfig<'a> { /// Input mappings (SubDevice -> MainDevice). - pub inputs: &'a [SyncManagerAssignment<'a>], + pub(crate) inputs: &'a [SyncManagerAssignment<'a>], /// Output mappings (MainDevice -> SubDevice). - pub outputs: &'a [SyncManagerAssignment<'a>], + pub(crate) outputs: &'a [SyncManagerAssignment<'a>], +} + +impl<'a> MappingConfig<'a> { + /// Create a new PDO mapping config with both inputs and outputs. + pub fn new( + inputs: &'a [SyncManagerAssignment<'a>], + outputs: &'a [SyncManagerAssignment<'a>], + ) -> Self { + Self { inputs, outputs } + } + + /// Create a new PDO mapping config with only inputs (SubDevice into MainDevice). + pub fn inputs(inputs: &'a [SyncManagerAssignment<'a>]) -> Self { + Self { + inputs, + outputs: &[], + } + } + + /// Create a new PDO mapping config with only outputs (MainDevice out to SubDevice). + pub fn outputs(outputs: &'a [SyncManagerAssignment<'a>]) -> Self { + Self { + inputs: &[], + outputs, + } + } } /// A typestate for [`SubDeviceGroup`] representing a group that is shut down. From 0e8c38e4c9695c063dc789b20fd2c009d90b1ba6 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 11:46:51 +0000 Subject: [PATCH 20/31] Make fields pub, but `#[non_exhaustive]` --- examples/el3702-oversampling.rs | 82 +++++++++++++++++---------------- src/ds402.rs | 12 +++-- src/subdevice_group/mod.rs | 5 +- 3 files changed, 52 insertions(+), 47 deletions(-) diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs index 0c29d30f..30fd7b82 100644 --- a/examples/el3702-oversampling.rs +++ b/examples/el3702-oversampling.rs @@ -104,46 +104,48 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); let el3702_mapping = MappingConfig::inputs( - &[ - SyncManagerAssignment::new( - const { - &[ - // Ch1 cycle count - PdoMapping::new( - 0x1b00, - const { &[PdoMapping::object::(0x6800, 1)] }, - ), - // Ch1 first sample - PdoMapping::new( - 0x1a00, - const { &[PdoMapping::object::(0x6000, 1)] }, - ) - .with_oversampling(2), - ] - }, - ) - .with_sync_manager(0) - .with_fmmu(0), - SyncManagerAssignment::new( - const { - &[ - // Ch1 cycle count - PdoMapping::new( - 0x1b00, - const { &[PdoMapping::object::(0x6800, 2)] }, - ), - // Ch1 first sample - PdoMapping::new( - 0x1a00, - const { &[PdoMapping::object::(0x6000, 2)] }, - ) - .with_oversampling(2), - ] - }, - ) - .with_sync_manager(1) - .with_fmmu(1), - ][..], + const { + &[ + SyncManagerAssignment::new( + const { + &[ + // Ch1 cycle count + PdoMapping::new( + 0x1b00, + const { &[PdoMapping::object::(0x6800, 1)] }, + ), + // Ch1 first sample + PdoMapping::new( + 0x1a00, + const { &[PdoMapping::object::(0x6000, 1)] }, + ) + .with_oversampling(2), + ] + }, + ) + .with_sync_manager(0) + .with_fmmu(0), + SyncManagerAssignment::new( + const { + &[ + // Ch1 cycle count + PdoMapping::new( + 0x1b00, + const { &[PdoMapping::object::(0x6800, 2)] }, + ), + // Ch1 first sample + PdoMapping::new( + 0x1a00, + const { &[PdoMapping::object::(0x6000, 2)] }, + ) + .with_oversampling(2), + ] + }, + ) + .with_sync_manager(1) + .with_fmmu(1), + ] + }, ); let group = group diff --git a/src/ds402.rs b/src/ds402.rs index 6b854a3a..0a28d43a 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -247,15 +247,16 @@ impl ReadObject { /// SDO config for a SubDevice's read (with [`ReadObject`]) or write (with [`WriteObject`]) PDOs. #[derive(Debug, Copy, Clone)] +#[non_exhaustive] pub struct SyncManagerAssignment<'a> { /// Sync manager index starting from 0. - pub(crate) sync_manager: Option, + pub sync_manager: Option, /// Desired FMMU. - pub(crate) fmmu: Option, + pub fmmu: Option, /// PDO mappings. - pub(crate) mappings: &'a [PdoMapping<'a>], + pub mappings: &'a [PdoMapping<'a>], } impl<'a> SyncManagerAssignment<'a> { @@ -316,14 +317,15 @@ impl<'a> SyncManagerAssignment<'a> { /// PDO object to be mapped. #[derive(Debug, Default, Copy, Clone)] +#[non_exhaustive] pub struct PdoMapping<'a> { /// PDO index, e.g. `0x1600` or `0x1a00`. pub index: u16, - /// PDO objects to map into this PDO. + /// Objects to map into this PDO. pub objects: &'a [u32], - /// Oversampling ratio. If in doubt, set to `None``. + /// Oversampling ratio. If in doubt, set to `None`. pub oversampling: Option, } diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 077f6180..db1db78c 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -69,12 +69,13 @@ impl MySyncUnsafeCell { /// TODO: Doc #[derive(Debug, Copy, Clone)] +#[non_exhaustive] pub struct MappingConfig<'a> { /// Input mappings (SubDevice -> MainDevice). - pub(crate) inputs: &'a [SyncManagerAssignment<'a>], + pub inputs: &'a [SyncManagerAssignment<'a>], /// Output mappings (MainDevice -> SubDevice). - pub(crate) outputs: &'a [SyncManagerAssignment<'a>], + pub outputs: &'a [SyncManagerAssignment<'a>], } impl<'a> MappingConfig<'a> { From d80cf11c8440e44e3ec12a240b62253cf34ca838 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 17 Mar 2025 12:47:01 +0000 Subject: [PATCH 21/31] Add method to write SDOs from config object --- examples/ds402.rs | 83 ++++++++++++---------------------- src/ds402.rs | 9 ++-- src/subdevice/configuration.rs | 9 +++- src/subdevice_group/mod.rs | 77 +++++++++++++++++++++++++++++-- 4 files changed, 116 insertions(+), 62 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index 0df3f0ca..a016991a 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -8,7 +8,7 @@ use ethercrab::{ ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, error::Error, std::ethercat_now, - subdevice_group::{CycleInfo, DcConfiguration, TxRxResponse}, + subdevice_group::{CycleInfo, DcConfiguration, MappingConfig, TxRxResponse}, DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, Timeouts, }; @@ -143,67 +143,44 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); - let outputs_config = [SyncManagerAssignment { - // Outputs, 0x1c12 - sync_manager: 2, - fmmu: 0, - mappings: &[PdoMapping { - index: 0x1600, - objects: &[ - // TODO: Could these become enums again? - ds402::WriteObject::CONTROL_WORD, - ds402::WriteObject::OP_MODE, - ], - ..PdoMapping::default() - }], - }]; - - let inputs_config = [SyncManagerAssignment { - // Inputs, 0x1c13 - sync_manager: 3, - fmmu: 1, - mappings: &[PdoMapping { - index: 0x1a00, - objects: &[ds402::ReadObject::STATUS_WORD, ds402::ReadObject::OP_MODE], - ..PdoMapping::default() - }], - }]; + let config = MappingConfig::new( + const { + &[SyncManagerAssignment::new( + const { + &[PdoMapping::new( + 0x1a00, + &[ds402::ReadObject::STATUS_WORD, ds402::ReadObject::OP_MODE], + )] + }, + )] + }, + const { + &[SyncManagerAssignment::new( + const { + &[PdoMapping::new( + 0x1600, + &[ + ds402::WriteObject::CONTROL_WORD, + ds402::WriteObject::OP_MODE, + ], + )] + }, + )] + }, + ); let group = group .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { if subdevice.name() == "PD4-EB59CD-E-65-1" { log::info!("Found servo {:?}", subdevice.identity()); - // subdevice - // .sdo_write_array( - // 0x1600, - // [ - // 0x6040_0010u32, // Control word, 16 bits - // 0x6060_0008, // Op mode, 8 bits - // ], - // ) - // .await?; - - // subdevice - // .sdo_write_array( - // 0x1a00, - // [ - // 0x6041_0010u32, // Status word, 16 bits - // 0x6061_0008, // Op mode reported - // ], - // ) - // .await?; - - // // Outputs to SubDevice - // subdevice.sdo_write_array(0x1c12, [0x1600u16]).await?; - - // // Inputs back to MainDevice - // subdevice.sdo_write_array(0x1c13, [0x1a00u16]).await?; + config.configure_sdos(&subdevice).await?; subdevice.set_dc_sync(DcSync::Sync0); - Ok(Some((&inputs_config[..], &outputs_config[..]))) - // Ok(None) + // Let EtherCrab configure the SubDevice automatically based on the SDOs we + // wrote just above. + Ok(None) } else { Ok(None) } diff --git a/src/ds402.rs b/src/ds402.rs index 0a28d43a..20c53018 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -224,13 +224,12 @@ pub enum Transition { #[derive(Copy, Clone)] pub struct WriteObject; -// TODO: Can this go back to being an enum? impl WriteObject { /// Control word. - pub const CONTROL_WORD: u32 = 0x6040_0010; + pub const CONTROL_WORD: u32 = PdoMapping::object::(0x6040, 0); /// Operation mode. - pub const OP_MODE: u32 = 0x6060_0008; + pub const OP_MODE: u32 = PdoMapping::object::(0x6060, 0); } /// An object received by the MainDevice from the SubDevice (TxPdo). @@ -239,10 +238,10 @@ pub struct ReadObject; impl ReadObject { /// Control word. - pub const STATUS_WORD: u32 = 0x6041_0010; + pub const STATUS_WORD: u32 = PdoMapping::object::(0x6041, 0); /// Operation mode. - pub const OP_MODE: u32 = 0x6061_0008; + pub const OP_MODE: u32 = PdoMapping::object::(0x6061, 0); } /// SDO config for a SubDevice's read (with [`ReadObject`]) or write (with [`WriteObject`]) PDOs. diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index 3568f814..697ae2b9 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -623,7 +623,7 @@ where .enumerate() .find(|(_idx, sm)| sm.usage_type == sm_type)?; - sync_managers.get(idx).map(|sm| (idx as u8, sm)) + Some((idx as u8, sm)) }) .ok_or_else(|| { fmt::error!( @@ -663,6 +663,13 @@ where }) }); + fmt::debug!( + "{:?} assignment SM {}, FMMU {}", + sm_type, + sync_manager_index, + fmmu_index + ); + let bit_len = assignment.len_bits(); let sm_config = self diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index db1db78c..ae6c4636 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -80,7 +80,7 @@ pub struct MappingConfig<'a> { impl<'a> MappingConfig<'a> { /// Create a new PDO mapping config with both inputs and outputs. - pub fn new( + pub const fn new( inputs: &'a [SyncManagerAssignment<'a>], outputs: &'a [SyncManagerAssignment<'a>], ) -> Self { @@ -88,7 +88,7 @@ impl<'a> MappingConfig<'a> { } /// Create a new PDO mapping config with only inputs (SubDevice into MainDevice). - pub fn inputs(inputs: &'a [SyncManagerAssignment<'a>]) -> Self { + pub const fn inputs(inputs: &'a [SyncManagerAssignment<'a>]) -> Self { Self { inputs, outputs: &[], @@ -96,12 +96,83 @@ impl<'a> MappingConfig<'a> { } /// Create a new PDO mapping config with only outputs (MainDevice out to SubDevice). - pub fn outputs(outputs: &'a [SyncManagerAssignment<'a>]) -> Self { + pub const fn outputs(outputs: &'a [SyncManagerAssignment<'a>]) -> Self { Self { inputs: &[], outputs, } } + + /// Write configuration to SubDevice CoE SDOs. + pub async fn configure_sdos( + &self, + subdevice: &SubDeviceRef<'_, &mut SubDevice>, + ) -> Result<(), Error> { + fmt::debug!("Write PDO mapping config"); + + for assignment in self.inputs.iter() { + for mapping in assignment.mappings.iter() { + fmt::debug!( + "--> Inputs {:#06x} {:#010x?}", + mapping.index, + mapping.objects + ); + + subdevice + .sdo_write_array(mapping.index, mapping.objects) + .await?; + } + } + + for assignment in self.outputs.iter() { + for mapping in assignment.mappings.iter() { + fmt::debug!( + "--> Outputs {:#06x} {:#010x?}", + mapping.index, + mapping.objects + ); + + subdevice + .sdo_write_array(mapping.index, mapping.objects) + .await?; + } + } + + for (idx, assignment) in self.inputs.iter().chain(self.outputs.iter()).enumerate() { + // First two SMs will be mailbox in/out if we have CoE support, so we start from 0x1c12 + // instead of 0x1c10. + let fallback = 2 + idx as u16; + + let sm_index = 0x1c10 + assignment.sync_manager.map(u16::from).unwrap_or(fallback); + + fmt::debug!("--> SM assignment {:#06x}", sm_index); + + subdevice.sdo_write(sm_index, 0, 0u8).await?; + + let mut count = 0u8; + + for (sub_index, mapping) in assignment.mappings.iter().enumerate() { + // Sub indices start at 8 + let sub_index = sub_index as u8 + 1; + + fmt::debug!( + "----> Object {:#06x} at sub-index {}", + mapping.index, + sub_index + ); + + subdevice + .sdo_write(sm_index, sub_index, mapping.index) + .await?; + + count += 1; + } + + subdevice.sdo_write(sm_index, 0, count).await?; + } + + Ok(()) + } } /// A typestate for [`SubDeviceGroup`] representing a group that is shut down. From e22ddea1bb699b3b2079d6ab35728ef63246b081 Mon Sep 17 00:00:00 2001 From: James Waples Date: Mon, 31 Mar 2025 11:15:33 +0100 Subject: [PATCH 22/31] Make a bit more progress on PDI mapping --- examples/ds402.rs | 436 +++++++++++++++++++++++-------------- src/ds402.rs | 175 +++++++-------- src/subdevice_group/mod.rs | 185 +++++++++++++++- 3 files changed, 543 insertions(+), 253 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index a016991a..fa437353 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -5,24 +5,26 @@ use env_logger::Env; use ethercrab::{ + DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, + RegisterAddress, Timeouts, ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, error::Error, std::ethercat_now, - subdevice_group::{CycleInfo, DcConfiguration, MappingConfig, TxRxResponse}, - DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, - RegisterAddress, Timeouts, + subdevice_group::{ + CycleInfo, DcConfiguration, MappingConfig, PdiMappingBikeshedName, TxRxResponse, + }, }; use futures_lite::StreamExt; use std::{ sync::{ - atomic::{AtomicBool, Ordering}, Arc, + atomic::{AtomicBool, Ordering}, }, thread, time::{Duration, Instant}, }; -use ta::indicators::ExponentialMovingAverage; use ta::Next; +use ta::indicators::ExponentialMovingAverage; /// Maximum number of SubDevices that can be stored. This must be a power of 2 greater than 1. const MAX_SUBDEVICES: usize = 16; @@ -143,6 +145,32 @@ fn main() -> Result<(), Error> { log::info!("Moving into PRE-OP with PDI"); + // Let's tackle the FMMU/SM config override thing later. Gonna focus on PDI mapping for now. + // enum SmUsage { + // // Direct values from ESI file. + // MBoxOut, + // MBoxIn, + // Outputs, + // Inputs, + // } + + // struct SmConfig { + // usage: SmUsage, + // size: Range, + // start_addr: u16, + // // TODO: A nice way of mapping `ControlByte`. Could I just ignore it and map based on SmUsage? + // // NOTE: Ignoring enable flag and just assuming always enabled + // } + + // // A subset of the information in an ESI file + // struct SubDeviceConfig<'a> { + // sync_managers: &'a [SmUsage], + // fmmus: &'a [FmmuConfig], + // io: MappingConfig<'a>, + // // TODO: Some way of assigning a default SM/FMMU for a `SyncManagerAssignment` of I or O + // // based on what the spec says should be the default. + // } + let config = MappingConfig::new( const { &[SyncManagerAssignment::new( @@ -169,221 +197,299 @@ fn main() -> Result<(), Error> { }, ); + let mut servo_mapping = None; + let group = group .into_pre_op_pdi_with_config(&maindevice, async |mut subdevice, idx| { if subdevice.name() == "PD4-EB59CD-E-65-1" { log::info!("Found servo {:?}", subdevice.identity()); + // This is required as the drive won't go into SAFE-OP without the SDOs + // configured. config.configure_sdos(&subdevice).await?; subdevice.set_dc_sync(DcSync::Sync0); + // Copy config and assign it to the subdevice by configured address. The rest of + // the subdevice isn't used here as it doesn't have a configured PDI yet + servo_mapping = Some(config.pdi_mapping(&subdevice)); + // Let EtherCrab configure the SubDevice automatically based on the SDOs we - // wrote just above. - Ok(None) + // wrote just above. The SM and FMMU config is read from a well-formed EEPROM. + // TODO: Need a way to tell EtherCrab to completely ignore the EEPROM for + // SM/FMMU assignment. + // TODO: Add a flag or something to tell EtherCrab to write SDO config or not. + // This data isn't defined in the ESI files AFAICS so maybe some heuristic like + // "has mailbox SM"? + Ok(Some(config)) } else { Ok(None) } }) .await?; - for sd in group.iter(&maindevice) { - log::info!( - "--> {:#06x} PDI {} input bytes, {} output bytes", - sd.configured_address(), - sd.inputs_raw().len(), - sd.outputs_raw().len() - ); - } - - log::info!("Done. PDI available. Waiting for SubDevices to align"); - - let mut now = Instant::now(); - let start = Instant::now(); - - // Repeatedly send group PDI and sync frame to align all SubDevice clocks. We use an - // exponential moving average of each SubDevice's deviation from the EtherCAT System Time - // (the time in the DC reference SubDevice) and take the maximum deviation. When that is - // below 100us (arbitraily chosen value for this demo), we call the sync good enough and - // exit the loop. - loop { - group - .tx_rx_sync_system_time(&maindevice) - .await - .expect("TX/RX"); - - if now.elapsed() >= Duration::from_millis(25) { - now = Instant::now(); - - let mut max_deviation = 0; - - for (s1, ema) in group.iter(&maindevice).zip(averages.iter_mut()) { - let diff = match s1 - .register_read::(RegisterAddress::DcSystemTimeDifference) - .await - { - Ok(value) => - // The returned value is NOT in two's compliment, rather the upper bit specifies - // whether the number in the remaining bits is odd or even, so we convert the - // value to `i32` using that logic here. - { - let flag = 0b1u32 << 31; - - if value >= flag { - // Strip off negative flag bit and negate value as normal - -((value & !flag) as i32) - } else { - value as i32 - } - } - Err(Error::WorkingCounter { .. }) => 0, - Err(e) => return Err(e), - }; - - let ema_next = ema.next(diff as f64); - - max_deviation = max_deviation.max(ema_next.abs() as u32); - } - - log::debug!("--> Max deviation {} ns", max_deviation); - - // Less than 100us max deviation - if max_deviation < 100_000 { - log::info!("Clocks settled after {} ms", start.elapsed().as_millis()); + // Now that we have a PDI and the SD has its configured offsets, we can wrap it in the PDI + // mapping + // Max 16 I and O mappings + let servo: PdiMappingBikeshedName<16, 16> = servo_mapping.expect("No servo"); + + let servo = servo + .with_subdevice(&maindevice, &group) + .expect("Didn't find SD in group"); + + { + loop { + // group.tx_rx().await; + + // We can still use the normal `SubDevice` stuff due to `Deref` magic + dbg!(servo.configured_address()); + + // TODO: How do we populate the return type for `input`? Right now we just have to + // assume the user will give the code the correct type. Maybe we just leave this + // as-is and rely on a derive in the future to figure it out from the ESI? What + // about &dyn traits in the config? + + // Supports tuples. If any one of the fields can't be found, an error is returned + let status: StatusWord = servo + .input(ds402::ReadObject::STATUS_WORD) + .expect("No mapping"); + // // Or without the error and just a panic: + // let status = + // servo.input_unchecked::(ds402::ReadObject::STATUS_WORD); + // // False if we try to set an object that wasn't mapped + // let exists = + // servo.set_output(ds402::WriteObject::CONTROL_WORD, ControlWord::whatever()); + + // // Just read value we're gonna send to the outputs + // let control = servo + // .output::(ds402::WriteObject::CONTROL_WORD) + // .expect("No mapping"); + // // TODO: `unchecked` variant - break; - } + break; } - - tick_interval.next().await; } - log::info!("Alignment done"); - - // SubDevice clocks are aligned. We can turn DC on now. - let group = group - .configure_dc_sync( - &maindevice, - DcConfiguration { - // Start SYNC0 100ms in the future - start_delay: Duration::from_millis(100), - // SYNC0 period should be the same as the process data loop in most cases - sync0_period: TICK_INTERVAL, - // Taken from ESI file - sync0_shift: Duration::from_nanos(250_000), - }, - ) - .await?; - let group = group .into_safe_op(&maindevice) .await - .expect("PRE-OP -> SAFE-OP"); + .expect("OP -> SAFE-OP"); - log::info!("SAFE-OP"); + // This shouldn't be possible because we moved the group! + // dbg!(servo.configured_address()); + + log::info!("OP -> SAFE-OP"); - // Request OP state without waiting for all SubDevices to reach it. Allows the immediate - // start of the process data cycle, which is required when DC sync is used, otherwise - // SubDevices never reach OP, most often timing out with a SyncManagerWatchdog error. let group = group - .request_into_op(&maindevice) + .into_pre_op(&maindevice) .await - .expect("SAFE-OP -> OP"); + .expect("SAFE-OP -> PRE-OP"); - log::info!("OP requested"); + log::info!("SAFE-OP -> PRE-OP"); - let op_request = Instant::now(); + let _group = group.into_init(&maindevice).await.expect("PRE-OP -> INIT"); - // Send PDI and check group state until all SubDevices enter OP state. At this point, we can - // exit this loop and enter the main process data loop that does not have the state check - // overhead present here. - loop { - let now = Instant::now(); + log::info!("PRE-OP -> INIT, shutdown complete"); - let response @ TxRxResponse { - working_counter: _wkc, - extra: CycleInfo { - next_cycle_wait, .. - }, - .. - } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + Ok(()) - if response.all_op() { - break; - } + // for sd in group.iter(&maindevice) { + // log::info!( + // "--> {:#06x} PDI {} input bytes, {} output bytes", + // sd.configured_address(), + // sd.inputs_raw().len(), + // sd.outputs_raw().len() + // ); + // } - smol::Timer::at(now + next_cycle_wait).await; - } + // log::info!("Done. PDI available. Waiting for SubDevices to align"); + + // let mut now = Instant::now(); + // let start = Instant::now(); + + // // Repeatedly send group PDI and sync frame to align all SubDevice clocks. We use an + // // exponential moving average of each SubDevice's deviation from the EtherCAT System Time + // // (the time in the DC reference SubDevice) and take the maximum deviation. When that is + // // below 100us (arbitraily chosen value for this demo), we call the sync good enough and + // // exit the loop. + // loop { + // group + // .tx_rx_sync_system_time(&maindevice) + // .await + // .expect("TX/RX"); + + // if now.elapsed() >= Duration::from_millis(25) { + // now = Instant::now(); + + // let mut max_deviation = 0; + + // for (s1, ema) in group.iter(&maindevice).zip(averages.iter_mut()) { + // let diff = match s1 + // .register_read::(RegisterAddress::DcSystemTimeDifference) + // .await + // { + // Ok(value) => + // // The returned value is NOT in two's compliment, rather the upper bit specifies + // // whether the number in the remaining bits is odd or even, so we convert the + // // value to `i32` using that logic here. + // { + // let flag = 0b1u32 << 31; + + // if value >= flag { + // // Strip off negative flag bit and negate value as normal + // -((value & !flag) as i32) + // } else { + // value as i32 + // } + // } + // Err(Error::WorkingCounter { .. }) => 0, + // Err(e) => return Err(e), + // }; + + // let ema_next = ema.next(diff as f64); + + // max_deviation = max_deviation.max(ema_next.abs() as u32); + // } + + // log::debug!("--> Max deviation {} ns", max_deviation); + + // // Less than 100us max deviation + // if max_deviation < 100_000 { + // log::info!("Clocks settled after {} ms", start.elapsed().as_millis()); + + // break; + // } + // } - log::info!( - "All SubDevices entered OP in {} us", - op_request.elapsed().as_micros() - ); + // tick_interval.next().await; + // } - let term = Arc::new(AtomicBool::new(false)); - signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term)) - .expect("Register hook"); + // log::info!("Alignment done"); + + // // SubDevice clocks are aligned. We can turn DC on now. + // let group = group + // .configure_dc_sync( + // &maindevice, + // DcConfiguration { + // // Start SYNC0 100ms in the future + // start_delay: Duration::from_millis(100), + // // SYNC0 period should be the same as the process data loop in most cases + // sync0_period: TICK_INTERVAL, + // // Taken from ESI file + // sync0_shift: Duration::from_nanos(250_000), + // }, + // ) + // .await?; + + // let group = group + // .into_safe_op(&maindevice) + // .await + // .expect("PRE-OP -> SAFE-OP"); + + // log::info!("SAFE-OP"); + + // // Request OP state without waiting for all SubDevices to reach it. Allows the immediate + // // start of the process data cycle, which is required when DC sync is used, otherwise + // // SubDevices never reach OP, most often timing out with a SyncManagerWatchdog error. + // let group = group + // .request_into_op(&maindevice) + // .await + // .expect("SAFE-OP -> OP"); + + // log::info!("OP requested"); + + // let op_request = Instant::now(); + + // // Send PDI and check group state until all SubDevices enter OP state. At this point, we can + // // exit this loop and enter the main process data loop that does not have the state check + // // overhead present here. + // loop { + // let now = Instant::now(); + + // let response @ TxRxResponse { + // working_counter: _wkc, + // extra: CycleInfo { + // next_cycle_wait, .. + // }, + // .. + // } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + + // if response.all_op() { + // break; + // } - let mut sd = group.subdevice(&maindevice, 0)?; + // smol::Timer::at(now + next_cycle_wait).await; + // } - // Main application process data cycle - loop { - let now = Instant::now(); + // log::info!( + // "All SubDevices entered OP in {} us", + // op_request.elapsed().as_micros() + // ); - let TxRxResponse { - working_counter: _wkc, - extra: CycleInfo { - next_cycle_wait, .. - }, - .. - } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + // let term = Arc::new(AtomicBool::new(false)); + // signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term)) + // .expect("Register hook"); - let io = sd.io_raw_mut(); + // let mut sd = group.subdevice(&maindevice, 0)?; - let i = io.inputs(); + // // Main application process data cycle + // loop { + // let now = Instant::now(); - let status_word = &i[0..2]; - let reported_op_mode = &i[2..3]; + // let TxRxResponse { + // working_counter: _wkc, + // extra: CycleInfo { + // next_cycle_wait, .. + // }, + // .. + // } = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); - let mut o = io.outputs(); + // let io = sd.io_raw_mut(); - let control_word = &mut o[0..2]; - let op_mode = &mut o[2..3]; + // let i = io.inputs(); - OpMode::CyclicSynchronousPosition.pack_to_slice(op_mode)?; + // let status_word = &i[0..2]; + // let reported_op_mode = &i[2..3]; - let status_word = StatusWord::unpack_from_slice(status_word)?; - let reported_op_mode = OpMode::unpack_from_slice(reported_op_mode)?; + // let mut o = io.outputs(); - // println!("Op mode {:?}", reported_op_mode); + // let control_word = &mut o[0..2]; + // let op_mode = &mut o[2..3]; - smol::Timer::at(now + next_cycle_wait).await; + // OpMode::CyclicSynchronousPosition.pack_to_slice(op_mode)?; - if term.load(Ordering::Relaxed) { - log::info!("Exiting..."); + // let status_word = StatusWord::unpack_from_slice(status_word)?; + // let reported_op_mode = OpMode::unpack_from_slice(reported_op_mode)?; - break; - } - } + // // println!("Op mode {:?}", reported_op_mode); - let group = group - .into_safe_op(&maindevice) - .await - .expect("OP -> SAFE-OP"); + // smol::Timer::at(now + next_cycle_wait).await; - log::info!("OP -> SAFE-OP"); + // if term.load(Ordering::Relaxed) { + // log::info!("Exiting..."); - let group = group - .into_pre_op(&maindevice) - .await - .expect("SAFE-OP -> PRE-OP"); + // break; + // } + // } - log::info!("SAFE-OP -> PRE-OP"); + // let group = group + // .into_safe_op(&maindevice) + // .await + // .expect("OP -> SAFE-OP"); - let _group = group.into_init(&maindevice).await.expect("PRE-OP -> INIT"); + // log::info!("OP -> SAFE-OP"); - log::info!("PRE-OP -> INIT, shutdown complete"); + // let group = group + // .into_pre_op(&maindevice) + // .await + // .expect("SAFE-OP -> PRE-OP"); - Ok(()) + // log::info!("SAFE-OP -> PRE-OP"); + + // let _group = group.into_init(&maindevice).await.expect("PRE-OP -> INIT"); + + // log::info!("PRE-OP -> INIT, shutdown complete"); + + // Ok(()) }) } diff --git a/src/ds402.rs b/src/ds402.rs index 20c53018..cdb890e0 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -4,7 +4,7 @@ use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite, EtherCrabWireSiz use heapless::FnvIndexMap; use libc::sync; -use crate::{fmt, SubDevicePdi, SubDeviceRef}; +use crate::{SubDevicePdi, SubDeviceRef, fmt}; /// DS402 control word (object 0x6040). /// @@ -361,6 +361,7 @@ impl<'a> PdoMapping<'a> { /// /// assert_eq!(raw, 0x6800_010f); /// ``` + // TODO: I think it'd be better to return an opaque type here pub const fn object(index: u16, subindex: u8) -> u32 where T: EtherCrabWireSized, @@ -399,11 +400,12 @@ impl<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> // TODO: Dynamically(?) compute let state_range = 0..StatusWord::PACKED_LEN; - fmt::unwrap_opt!(self - .subdevice - .inputs_raw() - .get(state_range) - .and_then(|bytes| StatusWord::unpack_from_slice(bytes).ok())) + fmt::unwrap_opt!( + self.subdevice + .inputs_raw() + .get(state_range) + .and_then(|bytes| StatusWord::unpack_from_slice(bytes).ok()) + ) } /// Get the current DS402 state machine state. @@ -419,84 +421,85 @@ mod tests { use super::*; use heapless::FnvIndexMap; - #[test] - fn raw() { - // SM configuration. Order matters! - // TODO: Make some fields mandatory: op mode, op mode status, supported drive modes. This is - // required by ETG6010 Table 8: Modes of operation – Object list - let outputs = &[SyncManagerAssignment { - // TODO: Higher level API so we can get the correct read/write SM address from the - // subdevice (e.g. `sd.read_sm(0) -> Option` or something) - // index: 0x1c12, - sync_manager: None, - - fmmu: None, - - // TODO: Validate that the SM can have this many mappings - mappings: &[PdoMapping { - index: 0x1600, - // TODO: Validate that this mapping object can have this many PDOs, e.g. some SD - // PDOs can only have 4 assignments - objects: &[WriteObject::CONTROL_WORD, WriteObject::OP_MODE], - oversampling: None, - }], - }]; - - // PDI offset accumulator - let mut position = 0; - - /// Convert SM config into a list of offsets into the SubDevice's PDI. - fn config_to_offsets( - config: &[SyncManagerAssignment], - position_accumulator: &mut usize, - ) -> FnvIndexMap, N> { - let mut map = FnvIndexMap::new(); - - // Turn nice mapping object into linear list of ranges into the SD's PDI - config - .into_iter() - .flat_map(|sm| sm.mappings) - .flat_map(|mapping| mapping.objects) - .for_each(|mapping| { - let object = *mapping; - - let size_bytes = (object & 0xffff) as usize / 8; - - let range = *position_accumulator..(*position_accumulator + size_bytes); - - *position_accumulator += size_bytes; - - let key = (object >> 16) as u16; - - assert_eq!( - map.insert(key, range), - Ok(None), - "multiple mappings of object {:#06x}", - key - ); - }); - - map - } - - let outputs = config_to_offsets(outputs, &mut position); - - let mut sd = Ds402::<32, 8> { - outputs, - // subdevice: SubDeviceRef::new(maindevice, 0x1000, SubDevicePdi::default()), - // FIXME: This is HILARIOUSLY unsafe but I really cba to mock up all the right stuff atm - subdevice: unsafe { MaybeUninit::zeroed().assume_init() }, - }; - - for (object, pdi_range) in sd.outputs.iter() { - println!( - "Object {:#06x}, {} PDI bytes at {:?}", - object, - pdi_range.len(), - pdi_range - ); - } - - sd.set_op_mode(OpMode::CyclicSynchronousPosition); - } + // #[test] + // fn raw() { + // // SM configuration. Order matters! + // // TODO: Make some fields mandatory: op mode, op mode status, supported drive modes. This is + // // required by ETG6010 Table 8: Modes of operation – Object list + // let outputs = &[SyncManagerAssignment { + // // TODO: Higher level API so we can get the correct read/write SM address from the + // // subdevice (e.g. `sd.read_sm(0) -> Option` or something) + // // index: 0x1c12, + // sync_manager: None, + + // fmmu: None, + + // // TODO: Validate that the SM can have this many mappings + // mappings: &[PdoMapping { + // index: 0x1600, + // // TODO: Validate that this mapping object can have this many PDOs, e.g. some SD + // // PDOs can only have 4 assignments + // objects: &[WriteObject::CONTROL_WORD, WriteObject::OP_MODE], + // oversampling: None, + // }], + // }]; + + // // PDI offset accumulator + // let mut position = 0; + + // /// Convert SM config into a list of offsets into the SubDevice's PDI. + // fn config_to_offsets( + // config: &[SyncManagerAssignment], + // position_accumulator: &mut usize, + // ) -> FnvIndexMap, N> { + // let mut map = FnvIndexMap::new(); + + // // Turn nice mapping object into linear list of ranges into the SD's PDI + // config + // .into_iter() + // .flat_map(|sm| sm.mappings) + // .flat_map(|mapping| mapping.objects) + // .for_each(|mapping| { + // let object = *mapping; + + // let size_bytes = (object & 0xff) as usize / 8; + + // let range = *position_accumulator..(*position_accumulator + size_bytes); + + // *position_accumulator += size_bytes; + + // // let key = (object >> 16) as u16; + // let key = object; + + // assert_eq!( + // map.insert(key, range), + // Ok(None), + // "multiple mappings of object {:#06x}", + // key + // ); + // }); + + // map + // } + + // let outputs = config_to_offsets(outputs, &mut position); + + // let mut sd = Ds402::<32, 8> { + // outputs, + // // subdevice: SubDeviceRef::new(maindevice, 0x1000, SubDevicePdi::default()), + // // FIXME: This is HILARIOUSLY unsafe but I really cba to mock up all the right stuff atm + // subdevice: unsafe { MaybeUninit::zeroed().assume_init() }, + // }; + + // for (object, pdi_range) in sd.outputs.iter() { + // println!( + // "Object {:#06x}, {} PDI bytes at {:?}", + // object, + // pdi_range.len(), + // pdi_range + // ); + // } + + // sd.set_op_mode(OpMode::CyclicSynchronousPosition); + // } } diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index ae6c4636..5319a933 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -21,8 +21,15 @@ use crate::{ }, timer_factory::IntoTimeout, }; -use core::{cell::UnsafeCell, marker::PhantomData, sync::atomic::AtomicUsize, time::Duration}; -use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireSized}; +use core::{ + cell::UnsafeCell, + marker::PhantomData, + ops::{Deref, Range}, + sync::atomic::AtomicUsize, + time::Duration, +}; +use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadSized, EtherCrabWireSized, WireError}; +use heapless::FnvIndexMap; pub use self::group_id::GroupId; pub use self::handle::SubDeviceGroupHandle; @@ -67,6 +74,88 @@ impl MySyncUnsafeCell { } } +// TODO: Un-pub +/// TODO: Docs +pub struct PdiMappingBikeshedName { + /// TODO: Docs. + configured_address: u16, + /// TODO: Docs. + inputs: FnvIndexMap, I>, + /// TODO: Docs. + outputs: FnvIndexMap, O>, + + state: S, +} + +impl PdiMappingBikeshedName { + /// TODO: Docs + /// TODO: Way better name + pub fn with_subdevice< + 'maindevice, + 'group, + const MAX_SUBDEVICES: usize, + const MAX_PDI: usize, + DC, + >( + self, + maindevice: &'maindevice MainDevice<'maindevice>, + group: &'group SubDeviceGroup, + ) -> Result< + PdiMappingBikeshedName>>, + Error, + > { + let subdevice = + group.subdevice_by_configured_address(maindevice, self.configured_address)?; + + Ok(PdiMappingBikeshedName { + configured_address: self.configured_address, + inputs: self.inputs, + outputs: self.outputs, + state: subdevice, + }) + } +} + +impl Deref for PdiMappingBikeshedName { + type Target = S; + + fn deref(&self) -> &Self::Target { + &self.state + } +} + +impl<'maindevice, 'group, const I: usize, const O: usize, const MAX_PDI: usize> + PdiMappingBikeshedName>> +{ + // TODO: Don't take a `u32` but an actual Object type + /// TODO: Doc + pub fn input(&self, object: u32) -> Result + where + T: EtherCrabWireReadSized, + { + let size_bytes = (object & 0xff) as usize / 8; + + if size_bytes != T::PACKED_LEN { + // TODO: This should probably be an Error::Mapping(IncorrectSize) + return Err(Error::Internal); + } + + let range = self.inputs.get(&object).ok_or_else(|| { + // TODO: Return idk, Error::Mapping(NotFound) + Error::Internal + })?; + + let inputs = self.state.inputs_raw(); + + let bytes = inputs.get(range.clone()).ok_or_else(|| { + // TODO: Error::Mapping(OutOfRange) or something + Error::Internal + })?; + + Ok(T::unpack_from_slice(bytes)?) + } +} + /// TODO: Doc #[derive(Debug, Copy, Clone)] #[non_exhaustive] @@ -173,6 +262,59 @@ impl<'a> MappingConfig<'a> { Ok(()) } + + /// TODO: Doc + pub fn pdi_mapping( + &self, + subdevice: &impl Deref, + ) -> PdiMappingBikeshedName { + let mut inputs = FnvIndexMap::<_, _, I>::new(); + let mut outputs = FnvIndexMap::<_, _, O>::new(); + + fn push_mapping( + inputs: &mut FnvIndexMap, N>, + position_accumulator: usize, + object: &u32, + ) -> usize { + let object = *object; + + let size_bytes = (object & 0xffff) as usize / 8; + + let range = position_accumulator..(position_accumulator + size_bytes); + + assert_eq!( + inputs.insert(object, range), + Ok(None), + "multiple mappings of object {:#06x}", + object + ); + + position_accumulator + size_bytes + } + + self.inputs + .iter() + .flat_map(|sm| sm.mappings) + .flat_map(|mapping| mapping.objects) + .fold(0usize, |position_accumulator, object| { + push_mapping(&mut inputs, position_accumulator, object) + }); + + self.outputs + .iter() + .flat_map(|sm| sm.mappings) + .flat_map(|mapping| mapping.objects) + .fold(0usize, |position_accumulator, object| { + push_mapping(&mut outputs, position_accumulator, object) + }); + + PdiMappingBikeshedName { + configured_address: subdevice.configured_address(), + inputs, + outputs, + state: (), + } + } } /// A typestate for [`SubDeviceGroup`] representing a group that is shut down. @@ -1078,6 +1220,45 @@ where )) } + /// Borrow an individual SubDevice, found by configured address. + pub fn subdevice_by_configured_address<'maindevice, 'group>( + &'group self, + maindevice: &'maindevice MainDevice<'maindevice>, + configured_address: u16, + ) -> Result>, Error> { + let subdevice = self + .inner() + .subdevices + .iter() + .find(|sd| sd.configured_address() == configured_address) + .ok_or(Error::NotFound { + item: Item::SubDevice, + index: Some(usize::from(configured_address)), + })?; + + let io_ranges = subdevice.io_segments().clone(); + + let IoRanges { + input: input_range, + output: output_range, + } = &io_ranges; + + fmt::trace!( + "Get SubDevice {:#06x} IO ranges I: {}, O: {} (group PDI {} byte subset of {} byte max)", + subdevice.configured_address(), + input_range, + output_range, + self.pdi_len, + MAX_PDI + ); + + Ok(SubDeviceRef::new( + maindevice, + subdevice.configured_address(), + SubDevicePdi::new(subdevice, &self.pdi), + )) + } + /// Get an iterator over all SubDevices in this group. pub fn iter<'group, 'maindevice>( &'group self, From 1cff0c789cf1054a305bf8545dcfe02f33c9f076 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sat, 5 Apr 2025 21:17:36 +0100 Subject: [PATCH 23/31] WIP some ramblings in prose and code Trying to refactor all the config stuff to work with future ESI support. --- NOTES.md | 39 ++++++++++ src/ds402.rs | 16 +---- src/eeprom/types.rs | 24 ++++--- src/subdevice/configuration.rs | 128 +++++++++++---------------------- src/subdevice/eeprom.rs | 18 +++-- src/subdevice_group/mod.rs | 125 ++++++++++++++++++++++++++++---- src/sync_manager_channel.rs | 22 ++++-- 7 files changed, 236 insertions(+), 136 deletions(-) diff --git a/NOTES.md b/NOTES.md index 482ed081..1d0e0eb9 100644 --- a/NOTES.md +++ b/NOTES.md @@ -10,6 +10,45 @@ this register. SOEM calls `0x0990` `ECT_REG_DCSTART0` and also writes a 64 bit value into it. See `ethercatdc.c`, `ecx_dcsync0`. +# 2025-04-05 SM and FMMU config + +If SM AND FMMU config is not given, use defaults: + +``` +NOTE The Sync Manager channels shall be used in the following way: +Sync Manager channel 0: mailbox write +Sync Manager channel 1: mailbox read +Sync Manager channel 2: process data write +(may be used for process data read if no process data write supported) +Sync Manager channel 3: process data read + +If mailbox is not supported, it shall be used in the following way: +Sync Manager channel 0: process data write +(may be used for process data read if no process data write supported) +Sync Manager channel 1: process data read +``` + +The `SyncManager` section is mandatory in EEPROM unless the SD doesn't have any PD, then it's +optional but the SD might still have mailbox. + +Mailbox SMs don't have FMMUs! Man I wrote that code a long time ago... look at +`configure_mailbox_sms`. SM only. The ESI FMMU type `MBoxState` is literally that, nothing to do +with actual MBox comms. It looks like if no SMs are in the EEPROM the current code won't bother +configuring mailboxes. Should probably fix that to default to SM 0/1 for both SMs due to it being +optional in the spec, but that's a separate issue. + +Make the SM/FMMU fields from the config struct optional and compute them based on MBox yes/no and +inputs/outputs. For config-in-Rust users I imagine they'll leave them as `None` most of the time, +but this field allows ESI support later. The ESI file scopes the chosen SM to `TxPdo`/`RxPdo`, so +that's where the field in Rust should be defined. + +This makes the current arch kinda weird. We have a bunch of `SyncManagerAssignment`s which we're +relying on the index for, but this doesn't map to ESI files that nicely. Could always just process +the ESI file though... + +ESI file `TxPdo`/`RxPdo` do not specify an FMMU, only a (default) sync manager - they can be +reassigned by the user. + # Overriding SM and FMMU config A key problem is wanting to store the SD config on `SubDevice` before it's used, meaning we'd either diff --git a/src/ds402.rs b/src/ds402.rs index cdb890e0..198ea17f 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -249,11 +249,10 @@ impl ReadObject { #[non_exhaustive] pub struct SyncManagerAssignment<'a> { /// Sync manager index starting from 0. + /// + /// If this is set to `None`, the sync manager will be chosen automatically by EtherCrab. pub sync_manager: Option, - /// Desired FMMU. - pub fmmu: Option, - /// PDO mappings. pub mappings: &'a [PdoMapping<'a>], } @@ -269,7 +268,6 @@ impl<'a> SyncManagerAssignment<'a> { Self { mappings, sync_manager: None, - fmmu: None, } } @@ -281,16 +279,6 @@ impl<'a> SyncManagerAssignment<'a> { } } - /// Set an explicit FMMU index to use. - /// - /// This will be the same as the sync manager index most of the time. - pub const fn with_fmmu(self, fmmu: u8) -> Self { - Self { - fmmu: Some(fmmu), - ..self - } - } - pub(crate) fn len_bits(&self) -> u16 { self.mappings .iter() diff --git a/src/eeprom/types.rs b/src/eeprom/types.rs index 1236fdb9..647fbb54 100644 --- a/src/eeprom/types.rs +++ b/src/eeprom/types.rs @@ -484,12 +484,16 @@ impl SyncManager { } else { // Try to recover type by matching on other fields in the SM match (self.control.operation_mode, self.control.direction) { - (OperationMode::Normal, Direction::MasterRead) => SyncManagerType::ProcessDataRead, - (OperationMode::Normal, Direction::MasterWrite) => { + (OperationMode::ProcessData, Direction::MainDeviceRead) => { + SyncManagerType::ProcessDataRead + } + (OperationMode::ProcessData, Direction::MainDeviceWrite) => { SyncManagerType::ProcessDataWrite } - (OperationMode::Mailbox, Direction::MasterRead) => SyncManagerType::MailboxRead, - (OperationMode::Mailbox, Direction::MasterWrite) => SyncManagerType::MailboxWrite, + (OperationMode::Mailbox, Direction::MainDeviceRead) => SyncManagerType::MailboxRead, + (OperationMode::Mailbox, Direction::MainDeviceWrite) => { + SyncManagerType::MailboxWrite + } } } } @@ -927,7 +931,7 @@ mod tests { length: 0x0080, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterWrite, + direction: Direction::MainDeviceWrite, ecat_event_enable: true, dls_user_event_enable: true, watchdog_enable: false, @@ -945,7 +949,7 @@ mod tests { length: 0x0080, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterRead, + direction: Direction::MainDeviceRead, ecat_event_enable: true, dls_user_event_enable: true, watchdog_enable: false, @@ -962,8 +966,8 @@ mod tests { start_addr: 0x1180, length: 0x0006, control: Control { - operation_mode: OperationMode::Normal, - direction: Direction::MasterWrite, + operation_mode: OperationMode::ProcessData, + direction: Direction::MainDeviceWrite, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -980,8 +984,8 @@ mod tests { start_addr: 0x1480, length: 0x0006, control: Control { - operation_mode: OperationMode::Normal, - direction: Direction::MasterRead, + operation_mode: OperationMode::ProcessData, + direction: Direction::MainDeviceRead, ecat_event_enable: false, dls_user_event_enable: false, watchdog_enable: false, diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index 697ae2b9..fc963355 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -12,6 +12,7 @@ use crate::{ pdi::{PdiOffset, PdiSegment}, register::RegisterAddress, subdevice::types::{Mailbox, MailboxConfig}, + subdevice_group::MappingConfig, subdevice_state::SubDeviceState, sync_manager_channel::{Enable, SM_BASE_ADDRESS, SM_TYPE_ADDRESS, Status, SyncManagerChannel}, }; @@ -108,7 +109,7 @@ where mut global_offset: PdiOffset, group_start_address: u32, direction: PdoDirection, - config: Option<&[SyncManagerAssignment<'_>]>, + config: &Option>, ) -> Result { let eeprom = self.eeprom(); @@ -141,9 +142,11 @@ where ); let range = if let Some(config) = config { - self.configure_pdos_config(&sync_managers, direction, &mut global_offset, config) + todo!(); + self.configure_pdos_config(direction, &mut global_offset, config) .await? - } else if has_coe { + // } else if has_coe { + } else if false { self.configure_pdos_coe(&sync_managers, &fmmu_usage, direction, &mut global_offset) .await? } else { @@ -152,13 +155,13 @@ where }; match direction { - PdoDirection::MasterRead => { + PdoDirection::MainDeviceRead => { self.state.config.io.input = PdiSegment { bytes: (range.bytes.start - group_start_address as usize) ..(range.bytes.end - group_start_address as usize), }; } - PdoDirection::MasterWrite => { + PdoDirection::MainDeviceWrite => { self.state.config.io.output = PdiSegment { bytes: (range.bytes.start - group_start_address as usize) ..(range.bytes.end - group_start_address as usize), @@ -524,14 +527,14 @@ where let eeprom = self.eeprom(); let pdos = match direction { - PdoDirection::MasterRead => { + PdoDirection::MainDeviceRead => { let read_pdos = eeprom.maindevice_read_pdos().await?; fmt::trace!("SubDevice inputs PDOs {:#?}", read_pdos); read_pdos } - PdoDirection::MasterWrite => { + PdoDirection::MainDeviceWrite => { let write_pdos = eeprom.maindevice_write_pdos().await?; fmt::trace!("SubDevice outputs PDOs {:#?}", write_pdos); @@ -582,6 +585,13 @@ where .write_sm_config(sync_manager_index, sync_manager, (bit_len + 7) / 8) .await?; + fmt::debug!( + "{:?} assignment SM {}, FMMU {}", + sm_type, + sync_manager_index, + fmmu_index + ); + self.write_fmmu_config( bit_len, usize::from(fmmu_index), @@ -601,90 +611,32 @@ where /// Configure PDOs from a given config. async fn configure_pdos_config( &self, - sync_managers: &[SyncManager], direction: PdoDirection, offset: &mut PdiOffset, - config: &[SyncManagerAssignment<'_>], + config: &MappingConfig<'_>, ) -> Result { - let start_offset = *offset; - - let fmmu_sm_mappings = self.eeprom().fmmu_mappings().await?; - - let (sm_type, _fmmu_type) = direction.filter_terms(); - - for assignment in config { - let (sync_manager_index, sync_manager) = assignment - .sync_manager - .and_then(|idx| sync_managers.get(usize::from(idx)).map(|sm| (idx, sm))) - .or_else(|| { - // If SM is not explicitly set, find the first SM of the type we're looking for - let (idx, sm) = sync_managers - .iter() - .enumerate() - .find(|(_idx, sm)| sm.usage_type == sm_type)?; - - Some((idx as u8, sm)) - }) - .ok_or_else(|| { - fmt::error!( - "Failed to find sync manager {:?} for {:?}", - sm_type, - assignment - ); - - Error::Internal - })?; - - if sync_manager.usage_type != sm_type { - fmt::error!( - "Sync manager index {} type is incorrect: {:?}, expected {:?}", - sync_manager_index, - sync_manager.usage_type, - sm_type - ); - - return Err(Error::Internal); - } + let eeprom = self.eeprom(); - let fmmu_index = assignment.fmmu.unwrap_or_else(|| { - // If no FMMU was explicitly set, look for FMMU index using FMMU_EX section in - // EEPROM. If that fails, fall back to using the sync manager index as FMMU index. - fmmu_sm_mappings - .iter() - .find(|fmmu| fmmu.sync_manager == sync_manager_index) - .map(|fmmu| fmmu.sync_manager) - .unwrap_or_else(|| { - fmt::trace!( - "Could not find FMMU for PDO SM{} in EEPROM, using SM index to pick FMMU instead", - sync_manager_index, - ); - - sync_manager_index - }) - }); + let sync_managers = if !config.sync_managers.is_empty() { + fmt::unwrap!(heapless::Vec::from_slice(config.sync_managers)) + } else { + // Fall back to trying to read sync managers from EEPROM if none were specified in the + // config. + let eeprom_sms = eeprom.sync_managers().await?; - fmt::debug!( - "{:?} assignment SM {}, FMMU {}", - sm_type, - sync_manager_index, - fmmu_index - ); + // eeprom_sms + // todo!() + heapless::Vec::<_, 8>::new() + }; - let bit_len = assignment.len_bits(); + let objects = match direction { + PdoDirection::MainDeviceRead => config.inputs, + PdoDirection::MainDeviceWrite => config.outputs, + }; - let sm_config = self - .write_sm_config(sync_manager_index, sync_manager, (bit_len + 7) / 8) - .await?; + let start_offset = *offset; - self.write_fmmu_config( - bit_len, - usize::from(fmmu_index), - offset, - sm_type, - &sm_config, - ) - .await?; - } + todo!(); Ok(PdiSegment { bytes: start_offset.up_to(*offset), @@ -694,15 +646,17 @@ where #[derive(Copy, Clone)] pub enum PdoDirection { - MasterRead, - MasterWrite, + MainDeviceRead, + MainDeviceWrite, } impl PdoDirection { fn filter_terms(self) -> (SyncManagerType, FmmuUsage) { match self { - PdoDirection::MasterRead => (SyncManagerType::ProcessDataRead, FmmuUsage::Inputs), - PdoDirection::MasterWrite => (SyncManagerType::ProcessDataWrite, FmmuUsage::Outputs), + PdoDirection::MainDeviceRead => (SyncManagerType::ProcessDataRead, FmmuUsage::Inputs), + PdoDirection::MainDeviceWrite => { + (SyncManagerType::ProcessDataWrite, FmmuUsage::Outputs) + } } } } diff --git a/src/subdevice/eeprom.rs b/src/subdevice/eeprom.rs index 7ce24fcf..86596267 100644 --- a/src/subdevice/eeprom.rs +++ b/src/subdevice/eeprom.rs @@ -249,6 +249,9 @@ where Ok(SubDeviceIdentity::unpack_from_slice(&buf)?) } + /// Load sync managers from EEPROM. + /// + /// If no sync manager category could be found, the returned list will be empty. pub(crate) async fn sync_managers(&self) -> Result, Error> { let mut sync_managers = heapless::Vec::<_, 8>::new(); @@ -450,6 +453,9 @@ where } } + /// Get an iterator-like object over all items in a category. + /// + /// If the category cannot be found, this method will return success with an empty iterator. pub(crate) async fn items( &self, category: CategoryType, @@ -560,7 +566,7 @@ mod tests { length: 0x0400, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterWrite, + direction: Direction::MainDeviceWrite, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -573,7 +579,7 @@ mod tests { length: 0x0400, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterRead, + direction: Direction::MainDeviceRead, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -585,8 +591,8 @@ mod tests { start_addr: 0x1100, length: 0x0000, control: Control { - operation_mode: OperationMode::Normal, - direction: Direction::MasterWrite, + operation_mode: OperationMode::ProcessData, + direction: Direction::MainDeviceWrite, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -598,8 +604,8 @@ mod tests { start_addr: 0x1140, length: 0x0000, control: Control { - operation_mode: OperationMode::Normal, - direction: Direction::MasterRead, + operation_mode: OperationMode::ProcessData, + direction: Direction::MainDeviceRead, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 5319a933..052b83a4 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -19,6 +19,7 @@ use crate::{ subdevice::{ IoRanges, SubDevice, SubDeviceRef, configuration::PdoDirection, pdi::SubDevicePdi, }, + sync_manager_channel::{Control, Direction, OperationMode}, timer_factory::IntoTimeout, }; use core::{ @@ -156,6 +157,87 @@ impl<'maindevice, 'group, const I: usize, const O: usize, const MAX_PDI: usize> } } +/// Sync Manager configuration. +#[derive(Debug, Copy, Clone)] +#[non_exhaustive] +pub struct SyncManagerConfig { + /// Start address. + /// + /// This is mandatory in ESI files. + pub start_addr: u16, + + /// Sync Manager default size. This MUST be set for mailbox sync managers. + /// + /// It will be computed and overwritten by EtherCrab for process data inputs/outputs based on + /// the mapping lengths. + pub size: u16, + + /// Configuration for this Sync Manager. + /// + /// This will be parsed from the `ControlByte` property in the ESI file. + pub control: Control, + + /// Whether to enable this sync manager or not. + pub enabled: bool, +} + +impl SyncManagerConfig { + /// Create a mailbox sync manager config with given size, start address and direction. + pub fn mailbox(start_addr: u16, size: u16, direction: Direction) -> Self { + SyncManagerConfig { + start_addr, + size, + enabled: true, + control: Control { + operation_mode: OperationMode::Mailbox, + direction, + ..Control::default() + }, + } + } + + /// Create a mailbox sync manager config with given size, start address and direction. + pub fn process_data(start_addr: u16, direction: Direction) -> Self { + SyncManagerConfig { + start_addr, + // Computed later when we do all the PDO mappings. + size: 0, + enabled: true, + control: Control { + operation_mode: OperationMode::ProcessData, + direction, + ..Control::default() + }, + } + } +} + +/// FMMU configuration. +#[derive(Debug, Copy, Clone)] +#[non_exhaustive] +pub struct Fmmu { + /// FMMU kind. + pub kind: FmmuKind, + + /// Sync manager index to assign to this FMMU. Leave as `None` if unsure. + pub sync_manager_index: Option, +} + +/// FMMU kind. +#[derive(Debug, Copy, Clone)] +pub enum FmmuKind { + /// Process data outputs from MainDevice. + Outputs, + /// Process data inputs into MainDevice. + Inputs, + /// Mailbox state. + MBoxState, + /// Dynamic inputs. + DynamicInputs, + /// Dynamic outputs. + DynamicOutputs, +} + /// TODO: Doc #[derive(Debug, Copy, Clone)] #[non_exhaustive] @@ -165,6 +247,18 @@ pub struct MappingConfig<'a> { /// Output mappings (MainDevice -> SubDevice). pub outputs: &'a [SyncManagerAssignment<'a>], + + /// FMMU configuration. + /// + /// When writing configuration manually, this field can be left empty (`&[]`) to let EtherCrab + /// compute sync manage assignments automatically. + pub fmmus: &'a [Fmmu], + + /// Sync manager config. + /// + /// When writing configuration manually, this field can be left empty (`&[]`) to let EtherCrab + /// compute sync manage assignments automatically. + pub sync_managers: &'a [SyncManagerConfig], } impl<'a> MappingConfig<'a> { @@ -173,7 +267,12 @@ impl<'a> MappingConfig<'a> { inputs: &'a [SyncManagerAssignment<'a>], outputs: &'a [SyncManagerAssignment<'a>], ) -> Self { - Self { inputs, outputs } + Self { + inputs, + outputs, + fmmus: &[], + sync_managers: &[], + } } /// Create a new PDO mapping config with only inputs (SubDevice into MainDevice). @@ -181,6 +280,8 @@ impl<'a> MappingConfig<'a> { Self { inputs, outputs: &[], + fmmus: &[], + sync_managers: &[], } } @@ -189,6 +290,8 @@ impl<'a> MappingConfig<'a> { Self { inputs: &[], outputs, + fmmus: &[], + sync_managers: &[], } } @@ -472,15 +575,13 @@ impl // Configure master read PDI mappings in the first section of the PDI for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { - let inputs_config = config.map(|c| c.inputs); - // We're in PRE-OP at this point pdi_position = SubDeviceRef::new(maindevice, subdevice.configured_address(), subdevice) .configure_fmmus( pdi_position, inner.pdi_start.start_address, - PdoDirection::MasterRead, - inputs_config, + PdoDirection::MainDeviceRead, + config, ) .await?; } @@ -493,8 +594,6 @@ impl // configure the write mappings in a separate loop. This means we have IIIIOOOO instead of // IOIOIO. for (subdevice, config) in inner.subdevices.iter_mut().zip(configs.iter()) { - let outputs_config = config.map(|c| c.outputs); - let addr = subdevice.configured_address(); let mut subdevice_config = SubDeviceRef::new(maindevice, addr, subdevice); @@ -504,8 +603,8 @@ impl .configure_fmmus( pdi_position, inner.pdi_start.start_address, - PdoDirection::MasterWrite, - outputs_config, + PdoDirection::MainDeviceWrite, + config, ) .await?; } @@ -550,8 +649,8 @@ impl .configure_fmmus( pdi_position, inner.pdi_start.start_address, - PdoDirection::MasterRead, - None, + PdoDirection::MainDeviceRead, + &None, ) .await?; } @@ -573,8 +672,8 @@ impl .configure_fmmus( pdi_position, inner.pdi_start.start_address, - PdoDirection::MasterWrite, - None, + PdoDirection::MainDeviceWrite, + &None, ) .await?; } diff --git a/src/sync_manager_channel.rs b/src/sync_manager_channel.rs index c0d16589..70040de9 100644 --- a/src/sync_manager_channel.rs +++ b/src/sync_manager_channel.rs @@ -59,18 +59,26 @@ impl core::fmt::Display for SyncManagerChannel { } } +/// Sync Manager config/control byte. +/// +/// ETG 1000.4 Table 58 Sync Manager Channel, relative offset 0x0004. #[derive(Default, Copy, Clone, Debug, PartialEq, Eq, ethercrab_wire::EtherCrabWireReadWrite)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[wire(bytes = 1)] pub struct Control { + /// Operation mode. #[wire(bits = 2)] pub operation_mode: OperationMode, + /// Direction (MainDevice -> SubDevice or SubDevice -> MainDevice) #[wire(bits = 2)] pub direction: Direction, + /// EtherCAT event enable. Leave `false` if unsure. #[wire(bits = 1)] pub ecat_event_enable: bool, + /// DLS-user event enable. Leave `false` if unsure. #[wire(bits = 1)] pub dls_user_event_enable: bool, + /// Sync manager watchdog enable. Leave `false` if unsure. #[wire(bits = 1, post_skip = 1)] pub watchdog_enable: bool, // reserved1: bool @@ -130,7 +138,7 @@ pub struct Enable { #[repr(u8)] pub enum OperationMode { #[default] - Normal = 0x00, + ProcessData = 0x00, Mailbox = 0x02, } @@ -139,9 +147,11 @@ pub enum OperationMode { #[wire(bits = 2)] #[repr(u8)] pub enum Direction { + /// Inputs to the MainDevice read from the SubDevice. #[default] - MasterRead = 0x00, - MasterWrite = 0x01, + MainDeviceRead = 0x00, + /// Outputs sent from the MainDevice to be output by the SubDevice. + MainDeviceWrite = 0x01, } /// Buffer state. @@ -184,7 +194,7 @@ mod tests { length_bytes: 0x0100, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterRead, + direction: Direction::MainDeviceRead, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -241,7 +251,7 @@ mod tests { parsed, Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterWrite, + direction: Direction::MainDeviceWrite, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, @@ -323,7 +333,7 @@ mod tests { length_bytes: 0x0080, control: Control { operation_mode: OperationMode::Mailbox, - direction: Direction::MasterWrite, + direction: Direction::MainDeviceWrite, ecat_event_enable: false, dls_user_event_enable: true, watchdog_enable: false, From f812a02e30e29e8db48754d0434c24f8e05e4453 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 6 Apr 2025 18:41:32 +0100 Subject: [PATCH 24/31] Write SM/FMMU configs from passed-in structs Requires as little EEPROM as possible --- NOTES.md | 2 + ...ing.rs => el3702-oversampling.rs.disabled} | 0 src/ds402.rs | 42 ++-- src/eeprom/types.rs | 55 ++--- src/subdevice/configuration.rs | 188 ++++++++++++++---- src/subdevice/eeprom.rs | 23 +-- src/subdevice_group/mod.rs | 27 ++- 7 files changed, 228 insertions(+), 109 deletions(-) rename examples/{el3702-oversampling.rs => el3702-oversampling.rs.disabled} (100%) diff --git a/NOTES.md b/NOTES.md index 1d0e0eb9..ff8ac1b5 100644 --- a/NOTES.md +++ b/NOTES.md @@ -49,6 +49,8 @@ the ESI file though... ESI file `TxPdo`/`RxPdo` do not specify an FMMU, only a (default) sync manager - they can be reassigned by the user. +ESI FMMU records can specify their associated sync manager. + # Overriding SM and FMMU config A key problem is wanting to store the SD config on `SubDevice` before it's used, meaning we'd either diff --git a/examples/el3702-oversampling.rs b/examples/el3702-oversampling.rs.disabled similarity index 100% rename from examples/el3702-oversampling.rs rename to examples/el3702-oversampling.rs.disabled diff --git a/src/ds402.rs b/src/ds402.rs index 198ea17f..297c2193 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -279,26 +279,28 @@ impl<'a> SyncManagerAssignment<'a> { } } - pub(crate) fn len_bits(&self) -> u16 { - self.mappings - .iter() - .map(|mapping| { - let mul = mapping.oversampling.unwrap_or(1).max(1); - - let sum: u16 = mapping - .objects - .iter() - .map(|object| { - // ETG1000.6 5.6.7.4.8; lower 8 bits are the object size - let size_bits = (object & 0xff) as u16; - - size_bits - }) - .sum(); - - sum * mul - }) - .sum() + pub(crate) fn object_sizes(&self) -> impl Iterator { + self.mappings.iter().map(|mapping| { + let mul = mapping.oversampling.unwrap_or(1).max(1); + + let sum: u16 = mapping + .objects + .iter() + .map(|object| { + // ETG1000.6 5.6.7.4.8; lower 8 bits are the object size + let size_bits = (object & 0xff) as u16; + + // Round up to nearest byte + (size_bits + 7) / 8 + }) + .sum(); + + sum * mul + }) + } + + pub(crate) fn len_bytes(&self) -> u16 { + self.object_sizes().sum() } } diff --git a/src/eeprom/types.rs b/src/eeprom/types.rs index 647fbb54..ee666f7f 100644 --- a/src/eeprom/types.rs +++ b/src/eeprom/types.rs @@ -464,37 +464,31 @@ pub struct SyncManager { #[wire(bytes = 2)] pub(crate) start_addr: u16, #[wire(bytes = 2)] - pub(crate) length: u16, + pub(crate) length_bytes: u16, #[wire(bytes = 1, post_skip_bytes = 1)] pub(crate) control: sync_manager_channel::Control, - #[wire(bytes = 1)] + #[wire(bytes = 1, post_skip_bytes = 1)] pub(crate) enable: SyncManagerEnable, - /// Usage type. - /// - /// Use the method of the same name instead of directly accessing this field. It is only exposed - /// for test purposes. - #[wire(bytes = 1)] - pub(crate) usage_type: SyncManagerType, + // /// Usage type. + // /// + // /// Use the method of the same name instead of directly accessing this field. It is only exposed + // /// for test purposes. + // #[wire(bytes = 1)] + // pub(crate) usage_type: SyncManagerType, } impl SyncManager { pub(crate) fn usage_type(&self) -> SyncManagerType { - if self.usage_type != SyncManagerType::Unknown { - self.usage_type - } else { - // Try to recover type by matching on other fields in the SM - match (self.control.operation_mode, self.control.direction) { - (OperationMode::ProcessData, Direction::MainDeviceRead) => { - SyncManagerType::ProcessDataRead - } - (OperationMode::ProcessData, Direction::MainDeviceWrite) => { - SyncManagerType::ProcessDataWrite - } - (OperationMode::Mailbox, Direction::MainDeviceRead) => SyncManagerType::MailboxRead, - (OperationMode::Mailbox, Direction::MainDeviceWrite) => { - SyncManagerType::MailboxWrite - } + // Try to recover type by matching on other fields in the SM + match (self.control.operation_mode, self.control.direction) { + (OperationMode::ProcessData, Direction::MainDeviceRead) => { + SyncManagerType::ProcessDataRead + } + (OperationMode::ProcessData, Direction::MainDeviceWrite) => { + SyncManagerType::ProcessDataWrite } + (OperationMode::Mailbox, Direction::MainDeviceRead) => SyncManagerType::MailboxRead, + (OperationMode::Mailbox, Direction::MainDeviceWrite) => SyncManagerType::MailboxWrite, } } } @@ -503,10 +497,9 @@ impl core::fmt::Debug for SyncManager { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { f.debug_struct("SyncManager") .field("start_addr", &format_args!("{:#06x}", self.start_addr)) - .field("length", &format_args!("{:#06x}", self.length)) + .field("length", &format_args!("{:#06x} bytes", self.length_bytes)) .field("control", &self.control) .field("enable", &self.enable) - .field("usage_type", &self.usage_type) .finish() } } @@ -928,7 +921,7 @@ mod tests { assert_eq!( SyncManager { start_addr: 0x1000, - length: 0x0080, + length_bytes: 0x0080, control: Control { operation_mode: OperationMode::Mailbox, direction: Direction::MainDeviceWrite, @@ -937,7 +930,6 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::Unknown, } .usage_type(), SyncManagerType::MailboxWrite @@ -946,7 +938,7 @@ mod tests { assert_eq!( SyncManager { start_addr: 0x10c0, - length: 0x0080, + length_bytes: 0x0080, control: Control { operation_mode: OperationMode::Mailbox, direction: Direction::MainDeviceRead, @@ -955,7 +947,6 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::Unknown, } .usage_type(), SyncManagerType::MailboxRead @@ -964,7 +955,7 @@ mod tests { assert_eq!( SyncManager { start_addr: 0x1180, - length: 0x0006, + length_bytes: 0x0006, control: Control { operation_mode: OperationMode::ProcessData, direction: Direction::MainDeviceWrite, @@ -973,7 +964,6 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::Unknown, } .usage_type(), SyncManagerType::ProcessDataWrite @@ -982,7 +972,7 @@ mod tests { assert_eq!( SyncManager { start_addr: 0x1480, - length: 0x0006, + length_bytes: 0x0006, control: Control { operation_mode: OperationMode::ProcessData, direction: Direction::MainDeviceRead, @@ -991,7 +981,6 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::Unknown, } .usage_type(), SyncManagerType::ProcessDataRead diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index fc963355..5a3d061c 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -1,10 +1,12 @@ +use heapless::Entry; + use super::{SubDevice, SubDeviceRef}; use crate::{ coe::{SdoExpedited, SubIndex}, ds402::SyncManagerAssignment, eeprom::types::{ - CoeDetails, DefaultMailbox, FmmuUsage, MailboxProtocols, SiiGeneral, SiiOwner, SyncManager, - SyncManagerEnable, SyncManagerType, + CoeDetails, DefaultMailbox, FmmuEx, FmmuUsage, MailboxProtocols, SiiGeneral, SiiOwner, + SyncManager, SyncManagerEnable, SyncManagerType, }, error::{Error, IgnoreNoCategory, Item}, fmmu::Fmmu, @@ -12,11 +14,11 @@ use crate::{ pdi::{PdiOffset, PdiSegment}, register::RegisterAddress, subdevice::types::{Mailbox, MailboxConfig}, - subdevice_group::MappingConfig, + subdevice_group::{FmmuConfig, FmmuKind, MappingConfig}, subdevice_state::SubDeviceState, sync_manager_channel::{Enable, SM_BASE_ADDRESS, SM_TYPE_ADDRESS, Status, SyncManagerChannel}, }; -use core::ops::DerefMut; +use core::{f32::consts::E, ops::DerefMut}; /// Configuation from EEPROM methods. impl SubDeviceRef<'_, S> @@ -142,11 +144,9 @@ where ); let range = if let Some(config) = config { - todo!(); self.configure_pdos_config(direction, &mut global_offset, config) .await? - // } else if has_coe { - } else if false { + } else if has_coe { self.configure_pdos_coe(&sync_managers, &fmmu_usage, direction, &mut global_offset) .await? } else { @@ -263,6 +263,9 @@ where let mut read_mailbox = None; let mut write_mailbox = None; + // NOTE: SOEM defaults SM0 to start 0x1000, size 0x0080 and SM1 to 0x1080/0x0080 if mailbox + // SMs can't be found. + for (sync_manager_index, sync_manager) in sync_managers.iter().enumerate() { let sync_manager_index = sync_manager_index as u8; @@ -447,8 +450,7 @@ where })?; self.write_fmmu_config( - sm_bit_len, - fmmu_index, + fmmu_index as u8, global_offset, desired_sm_type, &sm_config, @@ -467,15 +469,14 @@ where async fn write_fmmu_config( &self, - sm_bit_len: u16, - fmmu_index: usize, + fmmu_index: u8, global_offset: &mut PdiOffset, desired_sm_type: SyncManagerType, sm_config: &SyncManagerChannel, ) -> Result<(), Error> { // Multiple SMs may use the same FMMU, so we'll read the existing config from the SubDevice let mut fmmu_config = self - .read(RegisterAddress::fmmu(fmmu_index as u8)) + .read(RegisterAddress::fmmu(fmmu_index)) .receive::(self.maindevice) .await?; @@ -501,7 +502,7 @@ where } }; - self.write(RegisterAddress::fmmu(fmmu_index as u8)) + self.write(RegisterAddress::fmmu(fmmu_index)) .send(self.maindevice, &fmmu_config) .await?; @@ -512,7 +513,7 @@ where fmmu_config ); - *global_offset = global_offset.increment_byte_aligned(sm_bit_len); + *global_offset = global_offset.increment(sm_config.length_bytes); Ok(()) } @@ -546,7 +547,6 @@ where let fmmu_sm_mappings = eeprom.fmmu_mappings().await?; let start_offset = *offset; - // let mut total_bit_len = 0; let (sm_type, _fmmu_type) = direction.filter_terms(); @@ -557,14 +557,12 @@ where { let sync_manager_index = sync_manager_index as u8; - let bit_len = pdos + let bit_len: u16 = pdos .iter() .filter(|pdo| pdo.sync_manager == sync_manager_index) .map(|pdo| pdo.bit_len) .sum(); - // total_bit_len += bit_len; - // Look for FMMU index using FMMU_EX section in EEPROM. If it's empty, default // to looking through FMMU usage list and picking out the appropriate kind // (Inputs, Outputs) @@ -592,18 +590,11 @@ where fmmu_index ); - self.write_fmmu_config( - bit_len, - usize::from(fmmu_index), - offset, - sm_type, - &sm_config, - ) - .await?; + self.write_fmmu_config(fmmu_index, offset, sm_type, &sm_config) + .await?; } Ok(PdiSegment { - // bit_len: total_bit_len.into(), bytes: start_offset.up_to(*offset), }) } @@ -615,18 +606,39 @@ where offset: &mut PdiOffset, config: &MappingConfig<'_>, ) -> Result { + fmt::debug!( + "Configure SubDevice {:#06x} Sync Managers and FMMUs from given config", + self.configured_address() + ); + + let start_offset = *offset; + let eeprom = self.eeprom(); let sync_managers = if !config.sync_managers.is_empty() { - fmt::unwrap!(heapless::Vec::from_slice(config.sync_managers)) + config + .sync_managers + .iter() + .map(|sm| sm.bikeshed_into_eeprom_type()) + .collect() } else { // Fall back to trying to read sync managers from EEPROM if none were specified in the - // config. - let eeprom_sms = eeprom.sync_managers().await?; + // config. This list may be empty, in which case future code should map FMMUs and SMs by + // equal index. + eeprom.sync_managers().await? + }; - // eeprom_sms - // todo!() - heapless::Vec::<_, 8>::new() + let fmmu_sm_mappings = if !config.fmmus.is_empty() { + config + .fmmus + .iter() + .filter_map(|fmmu| fmmu.sync_manager.map(|sm| FmmuEx { sync_manager: sm })) + .collect() + } else { + // Fall back to trying to read sync managers from EEPROM if none were specified in the + // config. This list may be empty, in which case future code should map FMMUs and SMs by + // equal index. + eeprom.fmmu_mappings().await? }; let objects = match direction { @@ -634,9 +646,113 @@ where PdoDirection::MainDeviceWrite => config.outputs, }; - let start_offset = *offset; + let sm_type = match direction { + PdoDirection::MainDeviceRead => SyncManagerType::ProcessDataRead, + PdoDirection::MainDeviceWrite => SyncManagerType::ProcessDataWrite, + }; + + #[derive(Debug)] + struct ConfigBikeshed { + sync_manager: SyncManager, + fmmu_index: u8, + } - todo!(); + let mut configuration = heapless::FnvIndexMap::::new(); + + for (i, assignment) in objects.iter().enumerate() { + // SM index is either: + // - sm_config field which explicitly chooses one by index + // - If that's not populated, iterate and find an SM by type = PD and direction = the + // direction we've been given in the fn args. For multiple SMs with the same type, + // this will just find the first one but we can't do anything else. + let (sync_manager_index, sync_manager) = assignment + .sync_manager + .and_then(|sm_index| { + let sm = sync_managers.get(usize::from(sm_index))?; + + Some((sm_index, sm)) + }) + .or_else(|| { + sync_managers + .iter() + .enumerate() + .find(|(_idx, sm)| sm.usage_type() == sm_type) + .map(|(idx, sm)| (idx as u8, sm)) + }) + .ok_or_else(|| { + fmt::error!( + "Failed to find sync manager for {:?} assignment at index {}", + direction, + i + ); + + Error::NotFound { + item: Item::SyncManager, + index: Some(i), + } + })?; + + let fmmu_index = fmmu_sm_mappings + .iter() + .find(|fmmu| fmmu.sync_manager == sync_manager_index) + .map(|fmmu| fmmu.sync_manager) + .unwrap_or_else(|| { + fmt::trace!( + "Could not find FMMU for PDO SM{} in EEPROM, using SM index to pick FMMU{} instead", + sync_manager_index, + sync_manager_index, + ); + + sync_manager_index + }); + + // Takes into account oversampling if configured + let byte_len = assignment.len_bytes(); + + match configuration.entry(sync_manager_index) { + Entry::Occupied(mut config) => { + let config = config.get_mut(); + + // It should stay the same if everything is working correctly + debug_assert_eq!(config.fmmu_index, fmmu_index); + + // Just increment size + config.sync_manager.length_bytes += byte_len; + } + Entry::Vacant(vacant_entry) => { + let mut sync_manager = *sync_manager; + + sync_manager.length_bytes = byte_len; + + // The unwrap assumes the number of entries never goes above 16 here + fmt::unwrap!(vacant_entry.insert(ConfigBikeshed { + sync_manager, + fmmu_index, + })); + } + } + } + + for (sm_index, config) in configuration.into_iter() { + fmt::debug!( + "--> Configuring {:?} to SM{}, FMMU{}, {} bytes", + config.sync_manager.usage_type(), + sm_index, + config.fmmu_index, + config.sync_manager.length_bytes + ); + + let sm_config = self + .write_sm_config( + sm_index, + &config.sync_manager, + config.sync_manager.length_bytes, + ) + .await?; + + self.write_fmmu_config(config.fmmu_index, offset, sm_type, &sm_config) + .await?; + } Ok(PdiSegment { bytes: start_offset.up_to(*offset), @@ -644,7 +760,7 @@ where } } -#[derive(Copy, Clone)] +#[derive(Debug, Copy, Clone)] pub enum PdoDirection { MainDeviceRead, MainDeviceWrite, diff --git a/src/subdevice/eeprom.rs b/src/subdevice/eeprom.rs index 86596267..cf3bd424 100644 --- a/src/subdevice/eeprom.rs +++ b/src/subdevice/eeprom.rs @@ -252,8 +252,8 @@ where /// Load sync managers from EEPROM. /// /// If no sync manager category could be found, the returned list will be empty. - pub(crate) async fn sync_managers(&self) -> Result, Error> { - let mut sync_managers = heapless::Vec::<_, 8>::new(); + pub(crate) async fn sync_managers(&self) -> Result, Error> { + let mut sync_managers = heapless::Vec::<_, 16>::new(); fmt::trace!("Get sync managers"); @@ -532,7 +532,6 @@ mod tests { file_provider::EepromFile, types::{ CoeDetails, Flags, MailboxProtocols, PortStatus, PortStatuses, SyncManagerEnable, - SyncManagerType, }, }, sync_manager_channel::{Control, Direction, OperationMode}, @@ -563,7 +562,7 @@ mod tests { let expected = [ SyncManager { start_addr: 0x1800, - length: 0x0400, + length_bytes: 0x0400, control: Control { operation_mode: OperationMode::Mailbox, direction: Direction::MainDeviceWrite, @@ -572,11 +571,10 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::MailboxWrite, }, SyncManager { start_addr: 0x1c00, - length: 0x0400, + length_bytes: 0x0400, control: Control { operation_mode: OperationMode::Mailbox, direction: Direction::MainDeviceRead, @@ -585,11 +583,10 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::MailboxRead, }, SyncManager { start_addr: 0x1100, - length: 0x0000, + length_bytes: 0x0000, control: Control { operation_mode: OperationMode::ProcessData, direction: Direction::MainDeviceWrite, @@ -598,11 +595,10 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::ProcessDataWrite, }, SyncManager { start_addr: 0x1140, - length: 0x0000, + length_bytes: 0x0000, control: Control { operation_mode: OperationMode::ProcessData, direction: Direction::MainDeviceRead, @@ -611,13 +607,12 @@ mod tests { watchdog_enable: false, }, enable: SyncManagerEnable::ENABLE, - usage_type: SyncManagerType::ProcessDataRead, }, ]; assert_eq!( e.sync_managers().await, - Ok(heapless::Vec::::from_slice(&expected).unwrap()) + Ok(heapless::Vec::::from_slice(&expected).unwrap()) ); } @@ -879,7 +874,7 @@ mod tests { "subdevice_receive_offset" ); assert_eq!( - mbox.subdevice_receive_size, sms[0].length, + mbox.subdevice_receive_size, sms[0].length_bytes, "subdevice_receive_size" ); assert_eq!( @@ -887,7 +882,7 @@ mod tests { "subdevice_send_offset" ); assert_eq!( - mbox.subdevice_send_size, sms[1].length, + mbox.subdevice_send_size, sms[1].length_bytes, "subdevice_send_size" ); } diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 052b83a4..9185b668 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -12,6 +12,7 @@ use crate::{ al_control::AlControl, command::Command, ds402::SyncManagerAssignment, + eeprom::types::{SyncManager, SyncManagerEnable}, error::{DistributedClockError, Error, Item}, fmt, pdi::PdiOffset, @@ -19,7 +20,7 @@ use crate::{ subdevice::{ IoRanges, SubDevice, SubDeviceRef, configuration::PdoDirection, pdi::SubDevicePdi, }, - sync_manager_channel::{Control, Direction, OperationMode}, + sync_manager_channel::{Control, Direction, Enable, OperationMode}, timer_factory::IntoTimeout, }; use core::{ @@ -210,17 +211,31 @@ impl SyncManagerConfig { }, } } + + // TODO: Bikeshed: The `SyncManager` should probably not be an EEPROM-related type and just made generic. + pub(crate) fn bikeshed_into_eeprom_type(&self) -> crate::eeprom::types::SyncManager { + SyncManager { + start_addr: self.start_addr, + length_bytes: self.size, + control: self.control, + enable: if self.enabled { + SyncManagerEnable::ENABLE + } else { + SyncManagerEnable::empty() + }, + } + } } /// FMMU configuration. #[derive(Debug, Copy, Clone)] #[non_exhaustive] -pub struct Fmmu { +pub struct FmmuConfig { /// FMMU kind. pub kind: FmmuKind, /// Sync manager index to assign to this FMMU. Leave as `None` if unsure. - pub sync_manager_index: Option, + pub sync_manager: Option, } /// FMMU kind. @@ -251,13 +266,13 @@ pub struct MappingConfig<'a> { /// FMMU configuration. /// /// When writing configuration manually, this field can be left empty (`&[]`) to let EtherCrab - /// compute sync manage assignments automatically. - pub fmmus: &'a [Fmmu], + /// compute FMMU assignments automatically. + pub fmmus: &'a [FmmuConfig], /// Sync manager config. /// /// When writing configuration manually, this field can be left empty (`&[]`) to let EtherCrab - /// compute sync manage assignments automatically. + /// compute Sync Manager assignments automatically. pub sync_managers: &'a [SyncManagerConfig], } From d855baed2342031f1e78e8028a73181ab21fa199 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 6 Apr 2025 19:25:11 +0100 Subject: [PATCH 25/31] Start a bit more DS402 stuff --- examples/ds402.rs | 10 +-- src/ds402.rs | 140 +++++++++++++++++++++++++++++++------ src/subdevice_group/mod.rs | 8 +-- 3 files changed, 127 insertions(+), 31 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index fa437353..d304f87c 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -229,14 +229,16 @@ fn main() -> Result<(), Error> { .await?; // Now that we have a PDI and the SD has its configured offsets, we can wrap it in the PDI - // mapping - // Max 16 I and O mappings - let servo: PdiMappingBikeshedName<16, 16> = servo_mapping.expect("No servo"); + // mapping. This mapping is now bound to the subdevice we configured in + // `into_pre_op_pdi_with_config`. Max 16 I and O mappings + let servo_mapping: PdiMappingBikeshedName<16, 16> = servo_mapping.expect("No servo"); - let servo = servo + let servo = servo_mapping .with_subdevice(&maindevice, &group) .expect("Didn't find SD in group"); + let servo = Ds402::new(servo).expect("Could not create DS402"); + { loop { // group.tx_rx().await; diff --git a/src/ds402.rs b/src/ds402.rs index 297c2193..f18a5337 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -1,10 +1,17 @@ //! DS402/CiA402 high level interface. -use ethercrab_wire::{EtherCrabWireRead, EtherCrabWireReadWrite, EtherCrabWireSized}; +use core::ops::Range; + +use ethercrab_wire::{ + EtherCrabWireRead, EtherCrabWireReadWrite, EtherCrabWireSized, EtherCrabWireWrite, +}; use heapless::FnvIndexMap; +use io_uring::opcode::Read; use libc::sync; -use crate::{SubDevicePdi, SubDeviceRef, fmt}; +use crate::{ + SubDevicePdi, SubDeviceRef, error::Error, fmt, subdevice_group::PdiMappingBikeshedName, +}; /// DS402 control word (object 0x6040). /// @@ -361,39 +368,126 @@ impl<'a> PdoMapping<'a> { } /// Wrap a group SubDevice in a higher level DS402 API -pub struct Ds402<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> { - outputs: FnvIndexMap, MAX_OUTPUT_OBJECTS>, - // TODO: Inputs map - subdevice: SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, +pub struct Ds402< + 'group, + const MAX_PDI: usize, + const MAX_INPUT_OBJECTS: usize, + const MAX_OUTPUT_OBJECTS: usize, +> { + // inputs: FnvIndexMap, MAX_INPUT_OBJECTS>, + // outputs: FnvIndexMap, MAX_OUTPUT_OBJECTS>, + subdevice: PdiMappingBikeshedName< + MAX_INPUT_OBJECTS, + MAX_OUTPUT_OBJECTS, + SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, + >, + control_word: Range, + status_word: Range, + op_mode: Range, + op_mode_display: Range, } -impl<'group, const MAX_PDI: usize, const MAX_OUTPUT_OBJECTS: usize> - Ds402<'group, MAX_PDI, MAX_OUTPUT_OBJECTS> +impl<'group, const MAX_PDI: usize, const MAX_INPUT_OBJECTS: usize, const MAX_OUTPUT_OBJECTS: usize> + Ds402<'group, MAX_PDI, MAX_INPUT_OBJECTS, MAX_OUTPUT_OBJECTS> { - /// Set DS402 operation mode (CSV, CSP, etc). - // TODO: This will be a mandatory field at some point, so this specifically doesn't need - // to return a `Result`. - pub fn set_op_mode(&mut self, mode: OpMode) -> Result<(), ()> { - match self.outputs.get_mut(&((WriteObject::OP_MODE >> 16) as u16)) { - Some(v) => { - // v = mode; - todo!(); - - Ok(()) + /// Create a new DS402 instance with the given SubDevice. + pub fn new( + subdevice: PdiMappingBikeshedName< + MAX_INPUT_OBJECTS, + MAX_OUTPUT_OBJECTS, + SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, + >, + ) -> Result { + // Mandatory fields referenced from ETG6010 Table 85: Object Dictionary. The checks here are + // not a complete list of mandatory fields, just the ones we really want. E.g. we may want + // to add error register 0x1001 later. + for required in [ReadObject::STATUS_WORD, ReadObject::OP_MODE] { + if !subdevice.inputs.contains_key(&required) { + fmt::error!("Required read object {:#010x} not found", required); + + // TODO: Ds402Error::RequiredField or whatever + return Err(Error::Internal); + } + } + for required in [WriteObject::CONTROL_WORD, WriteObject::OP_MODE] { + if !subdevice.inputs.contains_key(&required) { + fmt::error!("Required write object {:#010x} not found", required); + + // TODO: Ds402Error::RequiredField or whatever + return Err(Error::Internal); } - None => Err(()), } + + let control_word = subdevice + .outputs + .get(&WriteObject::CONTROL_WORD) + .ok_or_else(|| { + fmt::error!( + "A mapping for DS402 Control Word ({:#010x}) must be provided", + WriteObject::CONTROL_WORD + ); + + Error::Internal + })? + .clone(); + let op_mode = subdevice + .outputs + .get(&WriteObject::OP_MODE) + .ok_or_else(|| { + fmt::error!( + "A mapping for DS402 Op Mode ({:#010x}) must be provided", + WriteObject::OP_MODE + ); + + Error::Internal + })? + .clone(); + + let status_word = subdevice + .outputs + .get(&ReadObject::STATUS_WORD) + .ok_or_else(|| { + fmt::error!( + "A mapping for DS402 Status Word ({:#010x}) must be provided", + ReadObject::STATUS_WORD + ); + + Error::Internal + })? + .clone(); + let op_mode_display = subdevice + .outputs + .get(&ReadObject::OP_MODE) + .ok_or_else(|| { + fmt::error!( + "A mapping for DS402 Op Mode Display ({:#010x}) must be provided", + ReadObject::OP_MODE + ); + + Error::Internal + })? + .clone(); + + Ok(Self { + subdevice, + control_word, + op_mode, + status_word, + op_mode_display, + }) + } + + /// Set DS402 operation mode (CSV, CSP, etc). + pub fn set_op_mode(&mut self, mode: OpMode) { + mode.pack_to_slice_unchecked(&mut self.subdevice.outputs_raw_mut()[self.op_mode.clone()]); } /// Get the DS402 status word. pub fn status_word(&self) -> StatusWord { - // TODO: Dynamically(?) compute - let state_range = 0..StatusWord::PACKED_LEN; - fmt::unwrap_opt!( self.subdevice .inputs_raw() - .get(state_range) + .get(self.status_word.clone()) .and_then(|bytes| StatusWord::unpack_from_slice(bytes).ok()) ) } diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 9185b668..91c849e9 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -80,13 +80,13 @@ impl MySyncUnsafeCell { /// TODO: Docs pub struct PdiMappingBikeshedName { /// TODO: Docs. - configured_address: u16, + pub(crate) configured_address: u16, /// TODO: Docs. - inputs: FnvIndexMap, I>, + pub(crate) inputs: FnvIndexMap, I>, /// TODO: Docs. - outputs: FnvIndexMap, O>, + pub(crate) outputs: FnvIndexMap, O>, - state: S, + pub(crate) state: S, } impl PdiMappingBikeshedName { From 8163bb23af335476c7bb7e7d700e7fe53032867c Mon Sep 17 00:00:00 2001 From: James Waples Date: Sat, 12 Apr 2025 19:58:21 +0100 Subject: [PATCH 26/31] Clarify usage a bit --- examples/ds402.rs | 34 ++++++++++++---------------------- src/ds402.rs | 11 +++++++++++ 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index d304f87c..58e8ba43 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -241,32 +241,22 @@ fn main() -> Result<(), Error> { { loop { - // group.tx_rx().await; + // We can call OG methods on the DS402 device using `.inner()` + // TODO: Decide if we want to do more deref chaining from `Ds402` -> + // `PdiMappingBikeshedName` -> `SubDeviceRef` -> `SubDevice` (lol) + dbg!(servo.inner().configured_address()); - // We can still use the normal `SubDevice` stuff due to `Deref` magic - dbg!(servo.configured_address()); - - // TODO: How do we populate the return type for `input`? Right now we just have to - // assume the user will give the code the correct type. Maybe we just leave this - // as-is and rely on a derive in the future to figure it out from the ESI? What - // about &dyn traits in the config? - - // Supports tuples. If any one of the fields can't be found, an error is returned + // We can drop down to reading/writing specific IO fields using `inner()` + // TODO: inner_mut() let status: StatusWord = servo + .inner() .input(ds402::ReadObject::STATUS_WORD) .expect("No mapping"); - // // Or without the error and just a panic: - // let status = - // servo.input_unchecked::(ds402::ReadObject::STATUS_WORD); - // // False if we try to set an object that wasn't mapped - // let exists = - // servo.set_output(ds402::WriteObject::CONTROL_WORD, ControlWord::whatever()); - - // // Just read value we're gonna send to the outputs - // let control = servo - // .output::(ds402::WriteObject::CONTROL_WORD) - // .expect("No mapping"); - // // TODO: `unchecked` variant + + // OR the preferred way: use higher level methods on the DS402 object. This doesn't + // panic because we made sure the status word mapping was present earlier. It may + // panic if the PDI isn't configured correctly though (too short, wrong range, etc). + let status: StatusWord = servo.status_word(); break; } diff --git a/src/ds402.rs b/src/ds402.rs index f18a5337..967017c8 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -496,6 +496,17 @@ impl<'group, const MAX_PDI: usize, const MAX_INPUT_OBJECTS: usize, const MAX_OUT pub fn state(&self) -> ReadState { self.status_word().state() } + + /// Get a reference to the underlying SubDevice with PDI. + pub fn inner( + &self, + ) -> &PdiMappingBikeshedName< + MAX_INPUT_OBJECTS, + MAX_OUTPUT_OBJECTS, + SubDeviceRef<'group, SubDevicePdi<'group, MAX_PDI>>, + > { + &self.subdevice + } } #[cfg(test)] From f568cc4ae975736c2bebab187dbad6efd1d9148d Mon Sep 17 00:00:00 2001 From: James Waples Date: Sun, 13 Apr 2025 10:32:45 +0100 Subject: [PATCH 27/31] Some thoughts --- examples/ds402.rs | 7 ++++++ src/ds402.rs | 60 ++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 59 insertions(+), 8 deletions(-) diff --git a/examples/ds402.rs b/examples/ds402.rs index 58e8ba43..baa668e0 100644 --- a/examples/ds402.rs +++ b/examples/ds402.rs @@ -259,6 +259,13 @@ fn main() -> Result<(), Error> { let status: StatusWord = servo.status_word(); break; + + // TODO: DS402 state machine API + // - Start with steady/transitioning/edge/whatever enum checks + // - How can we make this async and get the correct cycle's data? Will it always be + // the current one? Yes, as long as DS402 isn't spawned in a task parallel to + // TX/RX. The best way would be to have TX/RX and DS402 SM updates in one task, + // then logic in another } } diff --git a/src/ds402.rs b/src/ds402.rs index 967017c8..7499e59e 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -188,7 +188,7 @@ pub enum WriteState { /// This enum is created from the individual bits in or [`StatusWord`]. /// /// ETG6010 5.1 Figure 2: State Machine -#[derive(Debug, Copy, Clone)] +#[derive(Debug, Copy, Clone, PartialEq, Eq)] pub enum ReadState { /// Not ready to switch on. NotReadyToSwitchOn, @@ -211,13 +211,13 @@ pub enum ReadState { pub enum Transition { /// The device is in a steady state. Steady(ReadState), - /// The device is transitioning to a new desired state. - Transitioning { - /// Desired state. - desired: WriteState, - /// Current state. - actual: ReadState, - }, + // /// The device is transitioning to a new desired state. + // Transitioning { + // /// Desired state. + // desired: WriteState, + // /// Current state. + // actual: ReadState, + // }, /// The device has finished transitioning to a new state. Edge { /// Previous state before the transition started. @@ -367,6 +367,15 @@ impl<'a> PdoMapping<'a> { } } +/// TODO: Doc +#[derive(Debug, Copy, Clone)] +pub enum BikeshedHighLevelState { + /// TODO: Doc + Off, + /// TODO: Doc + On, +} + /// Wrap a group SubDevice in a higher level DS402 API pub struct Ds402< 'group, @@ -385,6 +394,8 @@ pub struct Ds402< status_word: Range, op_mode: Range, op_mode_display: Range, + desired_state: Option, + prev_state: ReadState, } impl<'group, const MAX_PDI: usize, const MAX_INPUT_OBJECTS: usize, const MAX_OUTPUT_OBJECTS: usize> @@ -474,9 +485,42 @@ impl<'group, const MAX_PDI: usize, const MAX_INPUT_OBJECTS: usize, const MAX_OUT op_mode, status_word, op_mode_display, + desired_state: None, + prev_state: ReadState::NotReadyToSwitchOn, }) } + /// TODO: Doc + pub fn bikeshed_set_high_level_state(&mut self, desired: BikeshedHighLevelState) { + self.desired_state = Some(desired) + } + + /// TODO: Doc + pub fn bikeshed_tick(&mut self) -> Transition { + // Special case for fault: the desired state is cleared and the end application must + // explicitly clear faults and set new desired state. + let new_state = if self.state() == ReadState::Fault { + self.desired_state = None; + + ReadState::Fault + } else { + todo!(); + }; + + let result = if new_state != self.prev_state { + Transition::Edge { + previous: self.prev_state, + current: new_state, + } + } else { + Transition::Steady(new_state) + }; + + self.prev_state = new_state; + + result + } + /// Set DS402 operation mode (CSV, CSP, etc). pub fn set_op_mode(&mut self, mode: OpMode) { mode.pack_to_slice_unchecked(&mut self.subdevice.outputs_raw_mut()[self.op_mode.clone()]); From 1b04c20031dc9d1ef7a3c9218f1a22c1d00918e6 Mon Sep 17 00:00:00 2001 From: James Waples Date: Sat, 17 May 2025 15:20:10 +0100 Subject: [PATCH 28/31] Add some PTP notes --- NOTES.md | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/NOTES.md b/NOTES.md index ff8ac1b5..d40ff330 100644 --- a/NOTES.md +++ b/NOTES.md @@ -10,6 +10,33 @@ this register. SOEM calls `0x0990` `ECT_REG_DCSTART0` and also writes a 64 bit value into it. See `ethercatdc.c`, `ecx_dcsync0`. +# 2025-04-30 PTP and synchronising DC from MainDevice + +Using PTP to sync a non-EtherCAT device like a Basler camera to the EtherCAT network. There needs to +be one reference clock, and seeing as Basler cameras have PTP support, we can use that to sync ECAT +time to the camera. + +Some details [here] +(https://events.static.linuxfound.org/sites/events/files/slides/lcjp14_ichikawa_0.pdf) and [here] +(https://docs.redhat.com/en/documentation/red_hat_enterprise_linux/7/html/system_administrators_guide/ch-Configuring_PTP_Using_ptp4l#sec-Starting_ptp4l). + +Basler PTP: + +```bash +sudo apt install linuxptp + +# Get some logs for an interface +sudo ptp4l -i enp115s0 -m +``` + +My PC already has `/dev/ptp0` but I'm not sure it's doing anything by default as the RedHat docs state: + +> When PTP time synchronization is working correctly, new messages with offsets and frequency +> adjustments are printed periodically to the ptp4l and phc2sys outputs if hardware time stamping +> is used. + +And I don't see new messages. + # 2025-04-05 SM and FMMU config If SM AND FMMU config is not given, use defaults: From 71988dd945c16e3d495900c2adad0381074e4d28 Mon Sep 17 00:00:00 2001 From: James Waples Date: Tue, 8 Jul 2025 16:15:22 +0100 Subject: [PATCH 29/31] Add some notes, plus fmt --- ...oversampling.rs.disabled => el3702-oversampling.rs} | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) rename examples/{el3702-oversampling.rs.disabled => el3702-oversampling.rs} (99%) diff --git a/examples/el3702-oversampling.rs.disabled b/examples/el3702-oversampling.rs similarity index 99% rename from examples/el3702-oversampling.rs.disabled rename to examples/el3702-oversampling.rs index 30fd7b82..01f3854f 100644 --- a/examples/el3702-oversampling.rs.disabled +++ b/examples/el3702-oversampling.rs @@ -5,24 +5,24 @@ use env_logger::Env; use ethercrab::{ + DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, + RegisterAddress, Timeouts, ds402::{self, Ds402, OpMode, PdoMapping, StatusWord, SyncManagerAssignment}, error::Error, std::ethercat_now, subdevice_group::{CycleInfo, DcConfiguration, MappingConfig, TxRxResponse}, - DcSync, EtherCrabWireRead, EtherCrabWireWrite, MainDevice, MainDeviceConfig, PduStorage, - RegisterAddress, Timeouts, }; use futures_lite::StreamExt; use std::{ sync::{ - atomic::{AtomicBool, Ordering}, Arc, + atomic::{AtomicBool, Ordering}, }, thread, time::{Duration, Instant}, }; -use ta::indicators::ExponentialMovingAverage; use ta::Next; +use ta::indicators::ExponentialMovingAverage; /// Maximum number of SubDevices that can be stored. This must be a power of 2 greater than 1. const MAX_SUBDEVICES: usize = 16; @@ -157,8 +157,10 @@ fn main() -> Result<(), Error> { sync1_period: Duration::from_millis(1), }); + // Set configuration using config struct Ok(Some(el3702_mapping)) } else { + // Autoconfigure from EEPROM or SDOs Ok(None) } }) From 12d90ff86db3ee02f981d8e60f0cacebae0ed9cb Mon Sep 17 00:00:00 2001 From: James Waples Date: Tue, 8 Jul 2025 16:35:00 +0100 Subject: [PATCH 30/31] Add `with_fmmu()` --- src/ds402.rs | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/ds402.rs b/src/ds402.rs index 7499e59e..528b8b6d 100644 --- a/src/ds402.rs +++ b/src/ds402.rs @@ -260,6 +260,9 @@ pub struct SyncManagerAssignment<'a> { /// If this is set to `None`, the sync manager will be chosen automatically by EtherCrab. pub sync_manager: Option, + /// TODO: Doc. + pub fmmu: Option, + /// PDO mappings. pub mappings: &'a [PdoMapping<'a>], } @@ -274,6 +277,7 @@ impl<'a> SyncManagerAssignment<'a> { pub const fn new(mappings: &'a [PdoMapping<'a>]) -> Self { Self { mappings, + fmmu: None, sync_manager: None, } } @@ -286,6 +290,15 @@ impl<'a> SyncManagerAssignment<'a> { } } + /// Set an explicit FMMU index to use. + // TODO: This isn't yet used in any of the configuration stuff + pub const fn with_fmmu(self, fmmu: u8) -> Self { + Self { + fmmu: Some(fmmu), + ..self + } + } + pub(crate) fn object_sizes(&self) -> impl Iterator { self.mappings.iter().map(|mapping| { let mul = mapping.oversampling.unwrap_or(1).max(1); @@ -306,6 +319,7 @@ impl<'a> SyncManagerAssignment<'a> { }) } + /// Return the length of the mappings in bytes, taking into account oversampling settings. pub(crate) fn len_bytes(&self) -> u16 { self.object_sizes().sum() } From cf95ead0c542c1d95b68be25c6ae5dac28e8f652 Mon Sep 17 00:00:00 2001 From: James Waples Date: Wed, 9 Jul 2025 08:48:14 +0100 Subject: [PATCH 31/31] Tweaks --- src/subdevice/configuration.rs | 1 + src/subdevice_group/mod.rs | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/subdevice/configuration.rs b/src/subdevice/configuration.rs index 5a3d061c..fea4c2d6 100644 --- a/src/subdevice/configuration.rs +++ b/src/subdevice/configuration.rs @@ -115,6 +115,7 @@ where ) -> Result { let eeprom = self.eeprom(); + // TODO: Don't read the EEPROM at all if we have a manual config let sync_managers = eeprom.sync_managers().await?; let fmmu_usage = eeprom.fmmus().await?; diff --git a/src/subdevice_group/mod.rs b/src/subdevice_group/mod.rs index 91c849e9..f269b9c1 100644 --- a/src/subdevice_group/mod.rs +++ b/src/subdevice_group/mod.rs @@ -771,7 +771,8 @@ impl self_.into_op(maindevice).await } - // NOTE: This just goes PRE-OP -> PRE-OP + PDI. What about into op and into safe op? + // NOTE: This just goes PRE-OP -> PRE-OP + PDI. What about the other methods like into op and + // into safe op, etc? /// TODO: Docs pub async fn into_pre_op_pdi_with_config<'fun>( mut self,