rewa81/qitech-control
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed Wago Initialization Logic

Browse Source 
fixed formatting
This commit is contained in:
kraemr committed 2026-01-30 16:16:10 +01:00
1 parent 093663c4e6
commit ff3e787a03
7 files changed
+333 -62
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

ethercat-hal/src/devices/mod.rs
+2 -1
View File
@@ -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,
ethercat-hal/src/devices/wago_750_354.rs
+121 -53
View File
@@ -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<Module>; 64],
pub slot_devices: [Option<Arc<RwLock<dyn DynamicEthercatDevice>>>; 64],
pub dev_count: usize,
pub module_count: usize,
rx_offsets: Vec<usize>,
tx_offsets: Vec<usize>,
rx_pdo_mappings: Vec<ModulePdoMapping>,
tx_pdo_mappings: Vec<ModulePdoMapping>,
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<usize> = vec![];
let mut vec: Vec<ModulePdoMapping> = 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::<u8>(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::<u8>(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::<u8>(index.0, index.1).await?;
let pdo_index = device.sdo_read::<u16>(index.0, 1).await?;
let pdo_map_count = device.sdo_read::<u8>(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<u32> = 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::<u8>(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;
ethercat-hal/src/devices/wago_modules/ip20_ec_di8_do8.rs
+3 -2
View File
@@ -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
ethercat-hal/src/devices/wago_modules/mod.rs
+1
View File
@@ -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;
ethercat-hal/src/devices/wago_modules/wago_750_430.rs
+203
View File
@@ -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<Module>,
}
#[derive(Debug, Clone)]
pub enum Wago750_430Port {
Port1,
Port2,
Port3,
Port4,
Port5,
Port6,
Port7,
Port8,
}
impl From<Wago750_430Port> 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<Wago750_430Port> for Wago750_430 {
fn get_input(
&self,
port: Wago750_430Port,
) -> Result<crate::io::digital_input::DigitalInputInput, anyhow::Error> {
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<u8, bitvec::prelude::Lsb0>,
) -> Result<(), anyhow::Error> {
Ok(())
}
fn input_len(&self) -> usize {
0
}
fn output(
&self,
output: &mut bitvec::prelude::BitSlice<u8, bitvec::prelude::Lsb0>,
) -> 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<u8, bitvec::prelude::Lsb0>,
) -> Result<(), anyhow::Error> {
Ok(())
}
fn output_checked(
&self,
_output: &mut bitvec::prelude::BitSlice<u8, bitvec::prelude::Lsb0>,
) -> Result<(), anyhow::Error> {
Ok(())
}
fn get_module(&self) -> Option<crate::devices::Module> {
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);
ethercat-hal/src/devices/wago_modules/wago_750_652.rs
+1 -4
View File
@@ -148,8 +148,7 @@ impl EthercatDevice for Wago750_652 {
&self,
output: &mut bitvec::prelude::BitSlice<u8, bitvec::prelude::Lsb0>,
) -> 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(())
}
server/src/ethercat/setup.rs
+2 -2
View File
@@ -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,