diff --git a/Cargo.lock b/Cargo.lock index 9aac6b93..7d4fcca9 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -968,8 +968,7 @@ dependencies = [ [[package]] name = "ethercrab" version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fbb13becfa15713adb09b27fdb74468b827cdc78aa99aea638a89c1b0db3ce94" +source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac" dependencies = [ "async-io", "atomic-waker", @@ -998,8 +997,7 @@ dependencies = [ [[package]] name = "ethercrab-wire" version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d15828cb020fb44c92ca67e1ee3c122c9621c2e00d383d066e488f0619023ba2" +source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac" dependencies = [ "ethercrab-wire-derive", "heapless", @@ -1008,8 +1006,7 @@ dependencies = [ [[package]] name = "ethercrab-wire-derive" version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e790b8011211ed70babfcacbd4277b91ee21402ad5ba6a2c293db02e2f12919" +source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac" dependencies = [ "criterion", "proc-macro2", @@ -2332,6 +2329,7 @@ dependencies = [ "smol", "socketioxide", "spin_sleep", + "ta", "textplots", "tokio", "tower-http", @@ -2588,6 +2586,12 @@ version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0bf256ce5efdfa370213c1dabab5935a12e49f2c58d15e9eac2870d3b4f27263" +[[package]] +name = "ta" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "609409d472a0a7d8d4dd9e19891bbdef546b9dce670c3057d0e02192dc541226" + [[package]] name = "tap" version = "1.0.1" diff --git a/control-core/Cargo.toml b/control-core/Cargo.toml index 36e94ca2..ecb72df0 100644 --- a/control-core/Cargo.toml +++ b/control-core/Cargo.toml @@ -13,7 +13,7 @@ axum = "0.8.6" ethercat_hal = { path = "../ethercat-hal" } units = { path = "../units" } -ethercrab = "0.6" +ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" } interfaces = "0.0.9" serde = "1.0.219" serialport = "4.7.3" diff --git a/ethercat-hal/Cargo.toml b/ethercat-hal/Cargo.toml index fcecfc72..3339c35e 100644 --- a/ethercat-hal/Cargo.toml +++ b/ethercat-hal/Cargo.toml @@ -8,7 +8,7 @@ workspace = true [dependencies] anyhow = "1.0.100" -ethercrab = "0.6" +ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" } ethercat_hal_derive = { version = "0.1.0", path = "../ethercat-hal-derive" } units = { path = "../units" } bitvec = { version = "1.0.1", features = ["alloc"] } diff --git a/ethercat-hal/src/devices/el5152.rs b/ethercat-hal/src/devices/el5152.rs index af6f7066..eac5f6fa 100644 --- a/ethercat-hal/src/devices/el5152.rs +++ b/ethercat-hal/src/devices/el5152.rs @@ -180,7 +180,6 @@ impl Configuration for EL5152Configuration { ) -> Result<(), anyhow::Error> { // Configure channel 1 self.channel1.write_channel_config(device, 0x8000).await?; - // Configure channel 2 self.channel2.write_channel_config(device, 0x8010).await?; // Write PDO assignments diff --git a/machines/Cargo.toml b/machines/Cargo.toml index 50634fea..cf2a1a57 100644 --- a/machines/Cargo.toml +++ b/machines/Cargo.toml @@ -4,7 +4,7 @@ version = "0.1.0" edition = "2024" [dependencies] -ethercrab = "0.6" +ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" } ethercat_hal = { path = "../ethercat-hal" } control_core = { path = "../control-core" } control_core_derive = { path = "../control-core-derive" } diff --git a/machines/src/aquapath1/new.rs b/machines/src/aquapath1/new.rs index d6f6c1f6..7dd6b3fb 100644 --- a/machines/src/aquapath1/new.rs +++ b/machines/src/aquapath1/new.rs @@ -1,7 +1,6 @@ use crate::{ MachineNewHardware, MachineNewParams, MachineNewTrait, get_ethercat_device, - get_subdevice_by_index, validate_no_role_dublicates, - validate_same_machine_identification_unique, + validate_no_role_dublicates, validate_same_machine_identification_unique, }; use super::{ @@ -92,7 +91,12 @@ impl MachineNewTrait for AquaPathV1 { pdo_assignment: EL5152PredefinedPdoAssignment::Frequency, ..Default::default() }; - let subdevice = get_subdevice_by_index(hardware.subdevices, 4)?; + + let subdevice = + get_ethercat_device::(hardware, params, 4, [EL5152_IDENTITY_A].to_vec()) + .await? + .1; + el5152 .write() .await diff --git a/server/Cargo.toml b/server/Cargo.toml index 3504986c..f558d10e 100644 --- a/server/Cargo.toml +++ b/server/Cargo.toml @@ -9,7 +9,7 @@ workspace = true [dependencies] -ethercrab = "0.6" +ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" } ethercat_hal = { path = "../ethercat-hal" } control_core = { path = "../control-core" } @@ -47,6 +47,7 @@ utils = { version = "0.1.0", path="../utils" } spin_sleep = "1.3.3" dhat = { version = "0.3.3", optional = true } ctrlc = { version = "3.3", optional = true } +ta = "0.5.0" [dev-dependencies] diff --git a/server/src/app_state.rs b/server/src/app_state.rs index 12b954fa..47ab3564 100644 --- a/server/src/app_state.rs +++ b/server/src/app_state.rs @@ -7,6 +7,7 @@ use anyhow::{Result, bail}; use control_core::socketio::event::GenericEvent; use ethercat_hal::devices::EthercatDevice; use ethercrab::SubDeviceRef; +use ethercrab::subdevice_group::HasDc; use ethercrab::{MainDevice, SubDeviceGroup, subdevice_group::Op}; use machines::machine_identification::{DeviceIdentification, MachineIdentificationUnique}; use machines::serial::registry::SERIAL_DEVICE_REGISTRY; @@ -102,22 +103,25 @@ pub struct EthercatSetup { /// All Ethercat devices /// Generic interface for all devices /// Needed to interface with the devices on an Ethercat level - pub group: SubDeviceGroup, + pub group: SubDeviceGroup, /// The Ethercat main device /// Needed to interface with the devices pub maindevice: MainDevice<'static>, + pub all_operational: bool, } impl EthercatSetup { pub fn new( devices: Vec<(DeviceIdentification, Arc>)>, - group: SubDeviceGroup, + group: SubDeviceGroup, maindevice: MainDevice<'static>, + all_operational: bool, ) -> Self { Self { devices, group, maindevice, + all_operational, } } } diff --git a/server/src/ethercat/setup.rs b/server/src/ethercat/setup.rs index 4d5d42d4..f3dd3678 100644 --- a/server/src/ethercat/setup.rs +++ b/server/src/ethercat/setup.rs @@ -18,10 +18,18 @@ use ethercat_hal::devices::wago_750_354::{ use ethercat_hal::devices::wago_modules::ip20_ec_di8_do8::{ IP20_EC_DI8_DO8_PRODUCT_ID, IP20_EC_DI8_DO8_VENDOR_ID, IP20EcDi8Do8, }; +use ethercrab::error::Error; +use ethercrab::subdevice_group::{DcConfiguration, HasDc, PreOpPdi, SafeOp}; +use smol::stream::StreamExt; +use ta::Next; +use ta::indicators::ExponentialMovingAverage; -use crate::utils::{start_dnsmasq, stop_dnsmasq}; +use crate::utils::stop_dnsmasq; use ethercrab::std::ethercat_now; -use ethercrab::{MainDevice, MainDeviceConfig, PduStorage, RetryBehaviour, Timeouts}; +use ethercrab::{ + DcSync, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, RetryBehaviour, + SubDeviceGroup, Timeouts, +}; use machines::machine_identification::{ DeviceHardwareIdentification, DeviceHardwareIdentificationEthercat, DeviceIdentification, DeviceIdentificationIdentified, MachineIdentificationUnique, read_device_identifications, @@ -30,6 +38,7 @@ use machines::registry::{MACHINE_REGISTRY, MachineRegistry}; use machines::{Machine, MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams}; use smol::channel::Sender; use socketioxide::extract::SocketRef; +use std::time::Instant; use std::{sync::Arc, time::Duration}; /// Structure to hold the result of grouping devices by identification @@ -172,7 +181,7 @@ pub async fn setup_loop( app_state: Arc, ) -> Result { tracing::info!("Starting Ethercat PDU loop"); - + const TICK_INTERVAL: Duration = Duration::from_micros(700); let res = stop_dnsmasq(); match res { Ok(_) => (), @@ -185,7 +194,6 @@ pub async fn setup_loop( let pdu_storage = Box::leak(Box::new(PduStorage::::new())); let (tx, rx, pdu) = pdu_storage.try_split().expect("can only split once"); let interface = interface.to_string(); - let mut has_dc = false; std::thread::Builder::new() .name("EthercatTxRxThread".to_owned()) @@ -278,6 +286,45 @@ pub async fn setup_loop( ))?, }; + for mut subdevice in group_preop.iter_mut(&maindevice) { + println!( + "subdevice {} {:X?}", + subdevice.name(), + subdevice.configured_address() + ); + if subdevice.name() == "EL4002" { + // Sync mode 01 = SM Synchronous, says it does dc but actually doesnt, thx? + subdevice + .sdo_write(0x1c32, 1, 1u16) + .await + .expect("Set sync mode"); + } + // Configure SYNC0 AND SYNC1 for EL4102 + else if subdevice.name() == "EL5152" { + // Sync mode 02 = SYNC0 + subdevice + .sdo_write(0x1c32, 1, 2u16) + .await + .expect("Set sync mode"); + + subdevice + .sdo_write(0x1c32, 0x02, TICK_INTERVAL.as_nanos() as u32) + .await + .expect("Set cycle time"); + + subdevice.set_dc_sync(DcSync::Sync01 { + // EL4102 ESI specifies SYNC1 with an offset of 100k ns + sync1_period: Duration::from_nanos(100_000), + }); + } else { + match subdevice.dc_support() { + ethercrab::DcSupport::None => (), + ethercrab::DcSupport::RefOnly => (), + ethercrab::DcSupport::Bits64 => subdevice.set_dc_sync(DcSync::Sync0), + ethercrab::DcSupport::Bits32 => subdevice.set_dc_sync(DcSync::Sync0), + } + } + } // create devices let devices = devices_from_subdevices::(&mut group_preop, &maindevice)?; @@ -298,6 +345,7 @@ pub async fn setup_loop( }, ) .collect::>(); + let devices = device_identifications .into_iter() .zip(devices) @@ -356,7 +404,6 @@ pub async fn setup_loop( */ match (coupler.identity().vendor_id, coupler.identity().product_id) { (WAGO_750_354_VENDOR_ID, WAGO_750_354_PRODUCT_ID) => { - has_dc = true; let r = Wago750_354::initialize_modules(coupler).await?; for module in r { if coupler.configured_address() == module.belongs_to_addr { @@ -380,7 +427,6 @@ pub async fn setup_loop( } } (IP20_EC_DI8_DO8_VENDOR_ID, IP20_EC_DI8_DO8_PRODUCT_ID) => { - has_dc = true; let r = IP20EcDi8Do8::initialize_modules(coupler).await?; for module in r { if coupler.configured_address() == module.belongs_to_addr { @@ -407,19 +453,11 @@ pub async fn setup_loop( }; drop(ethercat_meta_devices); - for subdevice in subdevices.iter() { - // Hack so El5152 goes into OP - if subdevice.name() == "EL5152" { - has_dc = true; - } - } - // remove subdevice from devices tuple let devices = devices .iter() .map(|(device_identification, device, _)| (device_identification.clone(), device.clone())) .collect::>(); - // Notify client via socketio set_ethercat_devices::( &identified_device_identifications, @@ -433,72 +471,127 @@ pub async fn setup_loop( ) .await?; - let group_safe = match group_preop.into_safe_op(&maindevice).await { - Ok(group_op) => { - tracing::info!("Group in Safe-OP state"); - group_op - } - Err(err) => Err(anyhow::anyhow!( - "[{}::setup_loop] Failed to put group in Safe-OP state: {:?}", - module_path!(), - err - ))?, - }; + // Avoid potential Sdo write Contention on the bus, by just waiting a bit + smol::Timer::after(Duration::from_millis(4000)).await; + let mut now = Instant::now(); + let start = Instant::now(); + let mut averages = Vec::new(); + for _ in 0..group_preop.len() { + averages.push(ExponentialMovingAverage::new(64).unwrap()); + } + let mut tick_interval = smol::Timer::interval(TICK_INTERVAL); - // TODO Make a more extensive init for the case of DC-Sync - // Maybe we need multiple groups? like one DC group and one non dc sync group? - // For now we just check if we use wago coupler or IP20 - if has_dc { - for _ in 1..1000 { - let res = group_safe.tx_rx_sync_system_time(&maindevice).await; - match res { - Ok(_) => (), - Err(e) => tracing::error!( - "[{}::setup_loop] Failed to sync dc time: {:?}", - e, - module_path!() - ), + tracing::info!("Moving into PRE-OP with PDI"); + let group = group_preop.into_pre_op_pdi(&maindevice).await?; + tracing::info!("Done. PDI available. Waiting for SubDevices to align"); + + 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(anyhow::anyhow!("Failed to read DC system time: {:?}", e)); + } + }; + + let ema_next = ema.next(diff as f64); + max_deviation = max_deviation.max(ema_next.abs() as u32); + } + if max_deviation < 100 { + tracing::info!("Clocks settled after {} ms", start.elapsed().as_millis()); + break; } } + tick_interval.next().await; } - // Put group in operational state - let group_op = match group_safe.into_op(&maindevice).await { - Ok(group_op) => { - tracing::info!("Group in OP state"); - group_op + // 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?; + + // State machine to handle transition to SafeOp with process data + enum GroupState { + PreOp(SubDeviceGroup), + SafeOp(SubDeviceGroup), + } + + let mut group_container = Some(GroupState::PreOp(group)); + let mut tick = 0; + + let group = loop { + let now = Instant::now(); + match group_container.take().unwrap() { + GroupState::PreOp(group) => { + let res = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + if tick > 300 { + let group = group.request_into_safe_op(&maindevice).await?; + group_container = Some(GroupState::SafeOp(group)); + tracing::info!("Requested SAFE-OP"); + } else { + group_container = Some(GroupState::PreOp(group)); + } + + smol::Timer::at(now + res.extra.next_cycle_wait).await; + } + GroupState::SafeOp(group) => { + let res = group.tx_rx_dc(&maindevice).await.expect("TX/RX"); + if res.all_safe_op() { + tracing::info!("SAFE-OP"); + break group; + } else { + group_container = Some(GroupState::SafeOp(group)); + } + smol::Timer::at(now + res.extra.next_cycle_wait).await; + } } - Err(err) => Err(anyhow::anyhow!( - "[{}::setup_loop] Failed to put group in OP state: {:?}", - module_path!(), - err - ))?, + tick += 1; }; - { - // Notify client via socketio - let app_state_clone = app_state.clone(); - let main_namespace = &mut app_state_clone - .socketio_setup - .namespaces - .write() - .await - .main_namespace; - let event = EthercatDevicesEventBuilder() - .build(app_state_clone.clone()) - .await; - main_namespace.emit(MainNamespaceEvents::EthercatDevicesEvent(event)); - } - - let res = start_dnsmasq(); - match res { - Ok(o) => o, - Err(e) => tracing::error!("Failed to start dnsmasq: {:?}", e), - }; + let group_op = group + .request_into_op(&maindevice) + .await + .expect("SAFE-OP -> OP"); + tracing::info!("Started Transition to OP"); Ok(EthercatSetup { devices, group: group_op, maindevice, + all_operational: false, }) } diff --git a/server/src/loop.rs b/server/src/loop.rs index f1d8100d..ca709283 100644 --- a/server/src/loop.rs +++ b/server/src/loop.rs @@ -4,6 +4,8 @@ use bitvec::prelude::*; use control_core::realtime::set_core_affinity; #[cfg(not(feature = "development-build"))] use control_core::realtime::set_realtime_priority; +use ethercrab::TxRxResponse; +use ethercrab::subdevice_group::CycleInfo; use machines::Machine; use machines::machine_identification::write_machine_device_identification; use smol::channel::Receiver; @@ -159,55 +161,79 @@ pub fn start_loop_thread( } pub async fn copy_ethercat_inputs( - ethercat_setup: Option<&EthercatSetup>, + ethercat_setup: Option<&mut EthercatSetup>, ) -> Result<(), anyhow::Error> { // only if we have an ethercat setup // - tx/rx cycle // - copy inputs to devices if let Some(ethercat_setup) = ethercat_setup { - ethercat_setup - .group - .tx_rx(ðercat_setup.maindevice) - .await?; + if ethercat_setup.all_operational { + ethercat_setup + .group + .tx_rx_dc(ðercat_setup.maindevice) + .await?; - // copy inputs to devices - for (i, subdevice) in ethercat_setup - .group - .iter(ðercat_setup.maindevice) - .enumerate() - { - // retrieve inputs - let input = subdevice.inputs_raw(); - let input_bits = input.view_bits::(); + // copy inputs to devices + for (i, subdevice) in ethercat_setup + .group + .iter(ðercat_setup.maindevice) + .enumerate() + { + // retrieve inputs + let input = subdevice.inputs_raw(); + let input_bits = input.view_bits::(); - // get device - let mut device = ethercat_setup.devices[i].1.as_ref().write().await; + // get device + let mut device = ethercat_setup.devices[i].1.as_ref().write().await; - // check if the device is used - if !device.is_used() { - // if the device is not used, we skip it - continue; + // check if the device is used + if !device.is_used() { + // if the device is not used, we skip it + continue; + } + + // put inputs into device + device.input_checked(input_bits).map_err(|e| { + anyhow::anyhow!( + "[{}::loop_once] SubDevice with index {} failed to copy inputs\n{:?}", + module_path!(), + i, + e + ) + })?; + + // post process inputs + device.input_post_process().map_err(|e| { + anyhow::anyhow!( + "[{}::loop_once] SubDevice with index {} failed to copy post_process\n{:?}", + module_path!(), + i, + e + ) + })?; + } + return Ok(()); + } else { + loop { + let now = Instant::now(); + let response @ TxRxResponse { + working_counter: _wkc, + extra: + CycleInfo { + next_cycle_wait, .. + }, + .. + } = ethercat_setup + .group + .tx_rx_dc(ðercat_setup.maindevice) + .await + .expect("TX/RX"); + if response.all_op() { + ethercat_setup.all_operational = true; + break; + } + smol::Timer::at(now + next_cycle_wait).await; } - - // put inputs into device - device.input_checked(input_bits).map_err(|e| { - anyhow::anyhow!( - "[{}::loop_once] SubDevice with index {} failed to copy inputs\n{:?}", - module_path!(), - i, - e - ) - })?; - - // post process inputs - device.input_post_process().map_err(|e| { - anyhow::anyhow!( - "[{}::loop_once] SubDevice with index {} failed to copy post_process\n{:?}", - module_path!(), - i, - e - ) - })?; } } Ok(()) @@ -276,7 +302,7 @@ pub fn loop_once<'maindevice>(inputs: &mut RtLoopInputs<'_>) -> Result<(), anyho .unwrap() .cycle_start(); - let res = smol::block_on(copy_ethercat_inputs(inputs.ethercat_setup.as_deref())); + let res = smol::block_on(copy_ethercat_inputs(inputs.ethercat_setup.as_deref_mut())); match res { Ok(_) => (), Err(e) => { diff --git a/server/src/main.rs b/server/src/main.rs index ce71dbd7..da44e3f3 100644 --- a/server/src/main.rs +++ b/server/src/main.rs @@ -2,6 +2,7 @@ use crate::{ metrics::collector::{RuntimeMetricsConfig, spawn_runtime_metrics_sampler}, socketio::main_namespace::machines_event::MachineObj, }; +use control_core::socketio::namespace::NamespaceCacheingLogic; use machines::{ AsyncThreadMessage, MachineConnection, MachineNewHardware, MachineNewHardwareSerial, MachineNewParams, SerialDevice, SerialDeviceIdentification, SerialDeviceNew, @@ -14,6 +15,9 @@ use machines::{ serial::{devices::laser::Laser, init::SerialDetection}, winder2::api::GenericEvent, }; +use socketio::main_namespace::{ + MainNamespaceEvents, ethercat_devices_event::EthercatDevicesEventBuilder, +}; #[cfg(feature = "development-build")] use std::sync::atomic::{AtomicBool, Ordering}; use utils::start_dnsmasq; @@ -145,8 +149,28 @@ pub async fn start_interface_discovery( Ok(setup) => { let _ = sender.send(HotThreadMessage::AddEtherCatSetup(setup)).await; tracing::info!("Successfully initialized EtherCAT devices"); - } + { + // Notify client via socketio + let app_state_clone = app_state.clone(); + let main_namespace = &mut app_state_clone + .socketio_setup + .namespaces + .write() + .await + .main_namespace; + let event = EthercatDevicesEventBuilder() + .build(app_state_clone.clone()) + .await; + main_namespace.emit(MainNamespaceEvents::EthercatDevicesEvent(event)); + } + + let res = start_dnsmasq(); + match res { + Ok(o) => o, + Err(e) => tracing::error!("Failed to start dnsmasq: {:?}", e), + }; + } Err(e) => { tracing::error!( "[{}::main] Failed to initialize EtherCAT network \n{:?}",