diff --git a/ethercat-hal/src/devices/mod.rs b/ethercat-hal/src/devices/mod.rs index e1b88f0a..26c4c895 100644 --- a/ethercat-hal/src/devices/mod.rs +++ b/ethercat-hal/src/devices/mod.rs @@ -56,8 +56,9 @@ use std::{any::Any, fmt::Debug, sync::Arc}; use wago_750_354::{WAGO_750_354_IDENTITY_A, Wago750_354}; use wago_modules::ip20_ec_di8_do8::{IP20_EC_DI8_DO8_IDENTITY, IP20EcDi8Do8}; -#[derive(Debug, Clone, Copy)] +#[derive(Debug, Clone)] pub struct Module { + pub name: String, pub slot: u16, pub belongs_to_addr: u16, pub has_tx: bool, diff --git a/ethercat-hal/src/devices/wago_750_354.rs b/ethercat-hal/src/devices/wago_750_354.rs index 6bb289ca..f2c0aade 100755 --- a/ethercat-hal/src/devices/wago_750_354.rs +++ b/ethercat-hal/src/devices/wago_750_354.rs @@ -2,6 +2,9 @@ use super::{ EthercatDevice, EthercatDeviceProcessing, EthercatDeviceUsed, NewEthercatDevice, SubDeviceIdentityTuple, }; +use crate::devices::wago_modules::wago_750_430::{ + WAGO_750_430_MODULE_IDENT, WAGO_750_430_PRODUCT_ID, +}; use crate::devices::wago_modules::*; use crate::{ devices::{ @@ -19,25 +22,28 @@ use crate::{ }; use anyhow::Error; use smol::lock::RwLock; +use std::collections::HashMap; use std::sync::Arc; - const MODULE_COUNT_INDEX: (u16, u8) = (0xf050, 0x00); const TX_MAPPING_INDEX: (u16, u8) = (0x1c13, 0x00); const RX_MAPPING_INDEX: (u16, u8) = (0x1c12, 0x00); -// For both the rx and tx The Wago Coupler has 4 bytes, which we dont care about and skip -/// Wago750_354 bus coupler -/* - The "Modules" simply write at an offset into rx and read at an offset in tx -*/ +#[derive(Clone, Debug)] +struct ModulePdoMapping { + pub offset: usize, + pub is_tx: bool, + pub module_i: u32, +} + +// For both the rx and tx The Wago Coupler has 4 bytes, which we dont care about and skip pub struct Wago750_354 { is_used: bool, pub slots: [Option; 64], pub slot_devices: [Option>>; 64], pub dev_count: usize, pub module_count: usize, - rx_offsets: Vec, - tx_offsets: Vec, + rx_pdo_mappings: Vec, + tx_pdo_mappings: Vec, tx_size: usize, rx_size: usize, } @@ -105,7 +111,7 @@ impl EthercatDevice for Wago750_354 { } fn set_module(&mut self, module: Module) { - self.slots[self.module_count] = Some(module); + self.slots[self.module_count] = Some(module.clone()); self.module_count += 1; self.tx_size += module.tx_offset; self.rx_size += module.rx_offset; @@ -134,8 +140,8 @@ impl NewEthercatDevice for Wago750_354 { dev_count: 0, tx_size: 0, rx_size: 0, - rx_offsets: vec![], - tx_offsets: vec![], + rx_pdo_mappings: vec![], + tx_pdo_mappings: vec![], } } } @@ -147,39 +153,77 @@ impl std::fmt::Debug for Wago750_354 { } impl Wago750_354 { + pub fn calculate_module_index(pdo_mapping: u32, is_tx: bool) -> u32 { + let start_index = match is_tx { + true => 0x6000 as u32, + false => 0x7000 as u32, + }; + let index_in_hex = ((pdo_mapping & 0xFFFF0000) >> 16) - start_index; + if index_in_hex < 16 { + return 0; + } else { + return index_in_hex / 16; + } + } + pub async fn get_pdo_offsets<'a>( &mut self, device: &EthercrabSubDevicePreoperational<'a>, get_tx: bool, ) -> Result<(), Error> { - let mut vec: Vec = vec![]; + let mut vec: Vec = vec![]; let mut bit_offset = 0; - let start_subindex = 0x1; + let mut module_i = 0; + let start_subindex = 0x2; + let index = match get_tx { true => (TX_MAPPING_INDEX.0, TX_MAPPING_INDEX.1), false => (RX_MAPPING_INDEX.0, RX_MAPPING_INDEX.1), }; - let count = device.sdo_read::(index.0, index.1).await?; - for i in 0..count { - vec.push(bit_offset); - let pdo_index = device.sdo_read(index.0, start_subindex + i).await?; - if pdo_index != 0 { - let pdo_map_count = device.sdo_read::(pdo_index, 0).await?; - // Iterate over every PDO Mapped entry and add it to the cumulative bit offset - for j in 0..pdo_map_count { - let pdo_mapping: u32 = device.sdo_read(pdo_index, 1 + j).await?; - // We only need / Want the bit len, which we extract with a bitmask extracting the lsb - let bit_length = (pdo_mapping & 0xFF) as u8; - bit_offset += bit_length as usize; - } - } + let count_mappings = device.sdo_read::(index.0, index.1).await?; + let pdo_index = device.sdo_read::(index.0, 1).await?; + let pdo_map_count = device.sdo_read::(pdo_index, 0).await?; + + for i in 0..pdo_map_count { + let pdo_mapping: u32 = device.sdo_read(pdo_index, 1 + i).await?; + let bit_length = (pdo_mapping & 0xFF) as u8; + bit_offset += bit_length as usize; } + let mut mappings_without_coupler: Vec = vec![]; + for i in start_subindex..count_mappings { + let pdo_index = device.sdo_read(index.0, i).await?; + let pdo_map_count = device.sdo_read::(pdo_index, 0).await?; + for j in 0..pdo_map_count { + let pdo_mapping: u32 = device.sdo_read(pdo_index, 1 + j).await?; + mappings_without_coupler.push(pdo_mapping); + } + } + mappings_without_coupler.sort(); + println!("{:?}", mappings_without_coupler); + + for pdo_mapping in mappings_without_coupler { + module_i = Wago750_354::calculate_module_index(pdo_mapping, get_tx); + let bit_length = (pdo_mapping & 0xFF) as u8; + if module_i < 64 { + vec.push(ModulePdoMapping { + offset: bit_offset, + is_tx: get_tx, + module_i, + }); + } + bit_offset += bit_length as usize; + } + + vec.sort_by_key(|e| (e.module_i, e.offset)); + // deduplicate by module_i, so we only have the offset to the start of inputs/outputs + vec.dedup_by(|a, b| a.module_i == b.module_i); + if get_tx { - self.tx_offsets = vec; + self.tx_pdo_mappings = vec; } else { - self.rx_offsets = vec; + self.rx_pdo_mappings = vec; } Ok(()) } @@ -222,6 +266,7 @@ impl Wago750_354 { has_rx: false, tx_offset: 0, rx_offset: 0, + name: "".to_string(), }; match ident_iom { @@ -229,26 +274,37 @@ impl Wago750_354 { WAGO_750_1506_PRODUCT_ID => { module.has_tx = true; module.has_rx = true; + module.name = "750-1506".to_string(); } WAGO_750_455_PRODUCT_ID => { module.has_tx = true; module.has_rx = false; + module.name = "750-455".to_string(); } WAGO_750_501_PRODUCT_ID => { module.has_tx = false; module.has_rx = true; + module.name = "750-501".to_string(); } WAGO_750_530_PRODUCT_ID => { module.has_tx = false; module.has_rx = true; + module.name = "750-530".to_string(); } WAGO_750_652_PRODUCT_ID => { module.has_tx = true; module.has_rx = true; + module.name = "750-652".to_string(); } WAGO_750_402_PRODUCT_ID => { module.has_tx = true; module.has_rx = false; + module.name = "750-402".to_string(); + } + WAGO_750_430_PRODUCT_ID => { + module.has_tx = true; + module.has_rx = false; + module.name = "750-430".to_string(); } _ => println!( "Wago-750-354 found Unknown/Unimplemented Module: {}", @@ -266,17 +322,42 @@ impl Wago750_354 { if self.dev_count != 0 { return; } - let mut tx_index = 1; - let mut rx_index = 1; smol::block_on(async { let _ = self.get_pdo_offsets(device, true).await; let _ = self.get_pdo_offsets(device, false).await; }); - println!("{:?}\n\n{:?}", self.tx_offsets, self.rx_offsets); + for module in &mut self.slots { + match module { + Some(m) => { + let tx_pdo_mapping = self + .tx_pdo_mappings + .iter() + .find(|map| map.module_i == m.slot.into()); + if m.has_tx { + m.tx_offset = match tx_pdo_mapping { + Some(map) => map.offset, + None => 0, + } + } - for module in self.slots { + let rx_pdo_mapping = self + .rx_pdo_mappings + .iter() + .find(|map| map.module_i == m.slot.into()); + if m.has_rx { + m.rx_offset = match rx_pdo_mapping { + Some(map) => map.offset, + None => 0, + } + } + } + None => break, + } + } + + for module in &self.slots { match module { Some(m) => { // Map ModuleIdent's to Terminals @@ -302,6 +383,10 @@ impl Wago750_354 { WAGO_750_402_MODULE_IDENT => { Arc::new(RwLock::new(wago_750_402::Wago750_402::new())) } + WAGO_750_430_MODULE_IDENT => { + Arc::new(RwLock::new(wago_750_430::Wago750_430::new())) + } + _ => { println!( "{} Missing Implementation for Module Identification: vendor_id: {:?}, module ident: {:?} !", @@ -312,27 +397,10 @@ impl Wago750_354 { return; } }; - - let tx_pdo_offset = self.tx_offsets.get(tx_index as usize); - if m.has_tx { - tx_index += 1; - } - - let rx_pdo_offset = self.rx_offsets.get(rx_index as usize); - if m.has_rx { - rx_index += 1; - } - let mut dev_guard = dev.write_blocking(); - match tx_pdo_offset { - Some(offset) => dev_guard.set_tx_offset(*offset), - None => (), - } - - match rx_pdo_offset { - Some(offset) => dev_guard.set_rx_offset(*offset), - None => (), - } + //println!("For {:?} setting tx: {} rx: {}",m.name,m.tx_offset,m.rx_offset); + dev_guard.set_tx_offset(m.tx_offset); + dev_guard.set_rx_offset(m.rx_offset); drop(dev_guard); self.slot_devices[self.dev_count] = Some(dev); self.dev_count += 1; diff --git a/ethercat-hal/src/devices/wago_modules/ip20_ec_di8_do8.rs b/ethercat-hal/src/devices/wago_modules/ip20_ec_di8_do8.rs index 451b8637..eccfb71e 100644 --- a/ethercat-hal/src/devices/wago_modules/ip20_ec_di8_do8.rs +++ b/ethercat-hal/src/devices/wago_modules/ip20_ec_di8_do8.rs @@ -214,7 +214,7 @@ impl EthercatDevice for IP20EcDi8Do8 { } fn set_module(&mut self, module: Module) { - self.slots[self.module_count] = Some(module); + self.slots[self.module_count] = Some(module.clone()); self.module_count += 1; self.tx_size += module.tx_offset; self.rx_size += module.rx_offset; @@ -378,6 +378,7 @@ impl IP20EcDi8Do8 { has_rx: false, tx_offset: 0, rx_offset: 0, + name: "".to_string(), }; match ident_iom { @@ -415,7 +416,7 @@ impl IP20EcDi8Do8 { let _ = self.get_pdo_offsets(device, false).await; }); - for module in self.slots { + for module in &self.slots { match module { Some(m) => { // Map ModuleIdent's to Terminals diff --git a/ethercat-hal/src/devices/wago_modules/mod.rs b/ethercat-hal/src/devices/wago_modules/mod.rs index ba84d125..21e48dc7 100644 --- a/ethercat-hal/src/devices/wago_modules/mod.rs +++ b/ethercat-hal/src/devices/wago_modules/mod.rs @@ -1,6 +1,7 @@ pub mod ip20_ec_di8_do8; pub mod wago_750_1506; pub mod wago_750_402; +pub mod wago_750_430; pub mod wago_750_455; pub mod wago_750_501; pub mod wago_750_530; diff --git a/ethercat-hal/src/devices/wago_modules/wago_750_430.rs b/ethercat-hal/src/devices/wago_modules/wago_750_430.rs new file mode 100644 index 00000000..41b33dd5 --- /dev/null +++ b/ethercat-hal/src/devices/wago_modules/wago_750_430.rs @@ -0,0 +1,203 @@ +use crate::devices::{ + DynamicEthercatDevice, EthercatDevice, EthercatDeviceUsed, EthercatDynamicPDO, Module, + SubDeviceProductTuple, +}; +use crate::devices::{EthercatDeviceProcessing, NewEthercatDevice}; +use crate::io::digital_input::{DigitalInputDevice, DigitalInputInput}; + +#[derive(Clone)] +pub struct Wago750_430 { + is_used: bool, + tx_bit_offset: usize, + rx_bit_offset: usize, + pub txpdo: Wago750_430TxPdo, + module: Option, +} + +#[derive(Debug, Clone)] +pub enum Wago750_430Port { + Port1, + Port2, + Port3, + Port4, + Port5, + Port6, + Port7, + Port8, +} + +impl From for usize { + fn from(value: Wago750_430Port) -> Self { + match value { + Wago750_430Port::Port1 => 0, + Wago750_430Port::Port2 => 1, + Wago750_430Port::Port3 => 2, + Wago750_430Port::Port4 => 3, + Wago750_430Port::Port5 => 4, + Wago750_430Port::Port6 => 5, + Wago750_430Port::Port7 => 6, + Wago750_430Port::Port8 => 7, + } + } +} + +#[derive(Clone, Default)] +pub struct Wago750_430TxPdo { + pub port1: bool, + pub port2: bool, + pub port3: bool, + pub port4: bool, + pub port5: bool, + pub port6: bool, + pub port7: bool, + pub port8: bool, +} + +impl DigitalInputDevice for Wago750_430 { + fn get_input( + &self, + port: Wago750_430Port, + ) -> Result { + match port { + Wago750_430Port::Port1 => Ok(DigitalInputInput { + value: self.txpdo.port1, + }), + Wago750_430Port::Port2 => Ok(DigitalInputInput { + value: self.txpdo.port2, + }), + Wago750_430Port::Port3 => Ok(DigitalInputInput { + value: self.txpdo.port3, + }), + Wago750_430Port::Port4 => Ok(DigitalInputInput { + value: self.txpdo.port4, + }), + Wago750_430Port::Port5 => Ok(DigitalInputInput { + value: self.txpdo.port5, + }), + Wago750_430Port::Port6 => Ok(DigitalInputInput { + value: self.txpdo.port6, + }), + Wago750_430Port::Port7 => Ok(DigitalInputInput { + value: self.txpdo.port7, + }), + Wago750_430Port::Port8 => Ok(DigitalInputInput { + value: self.txpdo.port8, + }), + } + } +} + +impl EthercatDeviceUsed for Wago750_430 { + fn is_used(&self) -> bool { + self.is_used + } + + fn set_used(&mut self, used: bool) { + self.is_used = used; + } +} + +impl DynamicEthercatDevice for Wago750_430 {} + +impl EthercatDynamicPDO for Wago750_430 { + fn get_tx_offset(&self) -> usize { + self.tx_bit_offset + } + + fn get_rx_offset(&self) -> usize { + self.rx_bit_offset + } + + fn set_tx_offset(&mut self, offset: usize) { + self.tx_bit_offset = offset + } + + fn set_rx_offset(&mut self, offset: usize) { + self.rx_bit_offset = offset + } +} + +impl EthercatDevice for Wago750_430 { + fn input( + &mut self, + _input: &bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + Ok(()) + } + + fn input_len(&self) -> usize { + 0 + } + + fn output( + &self, + output: &mut bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + Ok(()) + } + + fn output_len(&self) -> usize { + 0 + } + + fn as_any(&self) -> &dyn std::any::Any { + self + } + + fn as_any_mut(&mut self) -> &mut dyn std::any::Any { + self + } + + fn is_module(&self) -> bool { + true + } + + fn input_checked( + &mut self, + _input: &bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + Ok(()) + } + + fn output_checked( + &self, + _output: &mut bitvec::prelude::BitSlice, + ) -> Result<(), anyhow::Error> { + Ok(()) + } + + fn get_module(&self) -> Option { + self.module.clone() + } + + fn set_module(&mut self, module: crate::devices::Module) { + self.tx_bit_offset = module.tx_offset; + self.rx_bit_offset = module.rx_offset; + self.module = Some(module) + } +} + +impl EthercatDeviceProcessing for Wago750_430 {} + +impl NewEthercatDevice for Wago750_430 { + fn new() -> Self { + Self { + is_used: false, + tx_bit_offset: 0, + rx_bit_offset: 0, + module: None, + txpdo: Wago750_430TxPdo::default(), + } + } +} + +impl std::fmt::Debug for Wago750_430 { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "Wago750_430") + } +} + +pub const WAGO_750_430_VENDOR_ID: u32 = 0x00000021; +pub const WAGO_750_430_PRODUCT_ID: u32 = 2147483777; +pub const WAGO_750_430_MODULE_IDENT: SubDeviceProductTuple = + (WAGO_750_430_VENDOR_ID, WAGO_750_430_PRODUCT_ID); diff --git a/ethercat-hal/src/devices/wago_modules/wago_750_652.rs b/ethercat-hal/src/devices/wago_modules/wago_750_652.rs index 0f7da6f1..cb0acc42 100644 --- a/ethercat-hal/src/devices/wago_modules/wago_750_652.rs +++ b/ethercat-hal/src/devices/wago_modules/wago_750_652.rs @@ -148,8 +148,7 @@ impl EthercatDevice for Wago750_652 { &self, output: &mut bitvec::prelude::BitSlice, ) -> Result<(), anyhow::Error> { - let base = 32; - + let base = self.rx_bit_offset; let mut ol0_i = 5; // Ol0 starts at bit 5 and goes to bit 7 (inclusive) let mut ol1_i = 8; @@ -174,12 +173,10 @@ impl EthercatDevice for Wago750_652 { ); ol1_i += 1; } - println!("{}", self.rx_pdo.control.output_length); let data_start = base + 16; for (i, &byte) in self.rx_pdo.out_buffer.iter().take(22).enumerate() { output[(data_start + i * 8)..(data_start + (i + 1) * 8)].store_le(byte); } - //println!("{:?}",self.rx_pdo.out_buffer); Ok(()) } diff --git a/server/src/ethercat/setup.rs b/server/src/ethercat/setup.rs index b985bf1e..4d5d42d4 100644 --- a/server/src/ethercat/setup.rs +++ b/server/src/ethercat/setup.rs @@ -364,7 +364,7 @@ pub async fn setup_loop( Some(meta) => { let meta_data = EtherCatDeviceMetaData { configured_address: module.slot, - name: "wago module".to_owned(), + name:module.name, vendor_id:module.vendor_id, product_id: module.product_id, revision: 0x2, @@ -388,7 +388,7 @@ pub async fn setup_loop( Some(meta) => { let meta_data = EtherCatDeviceMetaData { configured_address: module.slot, - name: "IP20 EC DI8 DO8 module".to_owned(), + name:module.name, vendor_id:module.vendor_id, product_id: module.product_id, revision: 0x1,