Splitting groups to get aquapath working with winder.

Did not work.

control-core/src/ethercat/interface_discovery.rs

@@ -1,6 +1,5 @@
 use ethercrab::{
-    MainDevice, MainDeviceConfig, PduStorage, Timeouts,
-    std::{ethercat_now, tx_rx_task},
+    MainDevice, MainDeviceConfig, PduStorage, SubDeviceGroup, Timeouts, std::{ethercat_now, tx_rx_task}
 };
 use interfaces::Interface;
 use std::{process::Command, sync::Arc, time::Duration};
@@ -11,6 +10,13 @@ const IFACE_DISCOVERY_MAX_PDU_DATA: usize = PduStorage::element_size(1100);
 const IFACE_DISCOVERY_MAX_FRAMES: usize = 16;
 const IFACE_DISCOVERY_MAX_PDI_LEN: usize = 128;
 
+#[derive(Default)]
+pub struct Groups {
+    aquapath: SubDeviceGroup<IFACE_DISCOVERY_MAX_SUBDEVICES, IFACE_DISCOVERY_MAX_PDI_LEN>,
+    rest: SubDeviceGroup<IFACE_DISCOVERY_MAX_SUBDEVICES, IFACE_DISCOVERY_MAX_PDI_LEN>,
+}
+
+
 /// Sets a network interface to unmanaged by NetworkManager.
 /// Returns true if the command succeeded.
 pub fn set_interface_managed(interface: &str, managed: bool) -> bool {
@@ -97,7 +103,7 @@ pub async fn discover_ethercat_interface() -> Result<String, anyhow::Error> {
                 interface = Some(&interfaces[i].name);
                 break;
             }
-            Err(_) => (),
+            Err(e) => tracing::warn!("Error initializing interface {}: {}", interfaces[i].name, e),
         }
     }
 
@@ -119,7 +125,7 @@ pub async fn discover_ethercat_interface() -> Result<String, anyhow::Error> {
 }
 
 fn test_interface(interface: &str) -> Result<(), anyhow::Error> {
-    tracing::info!("Testing interface: {}", interface);
+    tracing::info!("Testing interface by OSH: {}", interface);
 
     let pdu_storage = Box::leak(Box::new(PduStorage::<
         IFACE_DISCOVERY_MAX_FRAMES,
@@ -137,6 +143,8 @@ fn test_interface(interface: &str) -> Result<(), anyhow::Error> {
                 .map_err(|e| anyhow::anyhow!("Failed to spawn TX/RX task: {}", e))?,
         );
 
+        println!("CREATING MAIN DEVICE");
+
         let maindevice = Arc::new(MainDevice::new(
             pdu_loop,
             Timeouts {
@@ -161,10 +169,29 @@ fn test_interface(interface: &str) -> Result<(), anyhow::Error> {
             },
         ));
 
+        println!("MAIN DEVICE CREATED");
+
         let result = maindevice
-            .init_single_group::<IFACE_DISCOVERY_MAX_SUBDEVICES, IFACE_DISCOVERY_MAX_PDI_LEN>(
-                ethercat_now,
-            )
+            .init::<IFACE_DISCOVERY_MAX_SUBDEVICES, _>(ethercat_now, |groups: &Groups, subdevice| {
+                panic!("GOT HERE");
+                println!("SUBDEVICE NAME {} ID {}", subdevice.name(), subdevice.identity());
+                if subdevice.name() == "EL5152" {
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    println!("USING AQUAPATH GROUP DUING INIT");
+                    return Ok(&groups.aquapath);
+                }
+
+                Ok(&groups.rest)
+            })
             .await
             .map(|_| ())
             .map_err(|e| anyhow::anyhow!("Failed to initialize group: {}", e));

machines/src/machine_identification.rs

@@ -2,6 +2,7 @@ use std::fmt::Display;
 
 use ethercat_hal::devices::wago_750_354::WAGO_750_354_IDENTITY_A;
 use ethercat_hal::devices::wago_modules::ip20_ec_di8_do8::IP20_EC_DI8_DO8_IDENTITY;
+use ethercrab::SubDevice;
 use serde::Deserialize;
 use serde::Serialize;
 
@@ -233,61 +234,40 @@ pub async fn read_device_identifications<'maindevice>(
     let mut result = Vec::new();
     for subdevice in subdevices.iter() {
         let identification = machine_device_identification(subdevice, maindevice).await;
-        result.push(identification);
+        result.push(identification.map_err(|e| anyhow!(
+            "[{}::machine_device_identification] Failed to read role from EEPROM for device {}: {}",
+            module_path!(),
+            subdevice.name(),
+            e
+        )));
     }
     result
 }
 
 /// Reads the machine device identification from the EEPROM
-pub async fn machine_device_identification<'maindevice>(
-    subdevice: &'maindevice EthercrabSubDevicePreoperational<'maindevice>,
+pub async fn machine_device_identification(
+    subdevice: &SubDevice,
     maindevice: &MainDevice<'_>,
-) -> Result<DeviceMachineIdentification, Error> {
-    let addresses = match get_identification_addresses(&subdevice.identity(), subdevice.name()) {
-        Ok(x) => x,
-        Err(e) => {
-            // u16dump(subdevice, maindevice, 0, 128).await?;
-            return Err(e);
-        }
-    };
+) -> Result<DeviceMachineIdentification, ethercrab::error::Error> {
+    let addresses = get_identification_addresses(&subdevice.identity(), subdevice.name())?;
 
     let mdi = DeviceMachineIdentification {
         machine_identification_unique: MachineIdentificationUnique {
             machine_identification: MachineIdentification {
                 vendor: subdevice
                 .eeprom_read::<u16>(maindevice, addresses.vendor_word)
-                .await
-                .or(Err(anyhow!(
-                    "[{}::machine_device_identification] Failed to read vendor from EEPROM for device {}",
-                    module_path!(),
-                    subdevice.name()
-                )))?,
+                .await?,
                 machine: subdevice
                 .eeprom_read::<u16>(maindevice, addresses.machine_word)
-                .await
-                .or(Err(anyhow!(
-                    "[{}::machine_device_identification] Failed to read machine from EEPROM for device {}",
-                    module_path!(),
-                    subdevice.name()
-                )))?,
+                .await?,
             },
             serial: subdevice
                 .eeprom_read::<u16>(maindevice, addresses.serial_word)
-                .await
-                .or(Err(anyhow!(
-                    "[{}::machine_device_identification] Failed to read serial from EEPROM for device {}",
-                    module_path!(),
-                    subdevice.name()
-                )))?,
+                .await?,
         },
         role: subdevice
             .eeprom_read::<u16>(maindevice, addresses.role_word)
-            .await
-            .or(Err(anyhow!(
-                "[{}::machine_device_identification] Failed to read role from EEPROM for device {}",
-                module_path!(),
-                subdevice.name()
-            )))?,
+            .await?,
     };
 
     tracing::debug!(
@@ -382,7 +362,7 @@ pub async fn write_machine_device_identification<'maindevice, const MAX_PDI: usi
 pub fn get_identification_addresses(
     subdevice_identity: &SubDeviceIdentity,
     subdevice_name: &str,
-) -> Result<MachineIdentificationAddresses, Error> {
+) -> Result<MachineIdentificationAddresses, ethercrab::error::Error> {
     let identity_tuple = subdevice_identity_to_tuple(subdevice_identity);
 
     Ok(match identity_tuple {
@@ -414,14 +394,14 @@ pub fn get_identification_addresses(
 
         _ => {
             // block_on(u16dump(&subdevice, maindevice, 0x00, 0xff))?;
-            Err(anyhow!(
-                "[{}::get_identification_addresses] Unknown MDI addresses for device {:?} vendor: 0x{:08x} product: 0x{:08x} revision: 0x{:08x}",
+            tracing::error!("[{}::get_identification_addresses] Unknown MDI addresses for device {:?} vendor: 0x{:08x} product: 0x{:08x} revision: 0x{:08x}",
                 module_path!(),
                 subdevice_name,
                 subdevice_identity.vendor_id,
                 subdevice_identity.product_id,
                 subdevice_identity.revision
-            ))?
+            );
+            return Err(ethercrab::error::Error::UnknownSubDevice);
         }
     })
 }

server/src/app_state.rs

@@ -1,4 +1,5 @@
 use crate::ethercat::config::{MAX_SUBDEVICES, PDI_LEN};
+use crate::ethercat::setup::Groups;
 use crate::rest::handlers::write_machine_device_identification::MachineDeviceInfoRequest;
 use crate::socketio::main_namespace::MainNamespaceEvents;
 use crate::socketio::main_namespace::machines_event::{MachineObj, MachinesEventBuilder};
@@ -102,7 +103,7 @@ pub struct EthercatSetup {
     /// All Ethercat devices
     /// Generic interface for all devices
     /// Needed to interface with the devices on an Ethercat level
-    pub group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op>,
+    pub groups: Groups<Op>,
     /// The Ethercat main device
     /// Needed to interface with the devices
     pub maindevice: MainDevice<'static>,
@@ -111,12 +112,12 @@ pub struct EthercatSetup {
 impl EthercatSetup {
     pub fn new(
         devices: Vec<(DeviceIdentification, Arc<RwLock<dyn EthercatDevice>>)>,
-        group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op>,
+        groups: Groups<Op>,
         maindevice: MainDevice<'static>,
     ) -> Self {
         Self {
             devices,
-            group,
+            groups,
             maindevice,
         }
     }

server/src/ethercat/setup.rs

@@ -11,23 +11,24 @@ use control_core::socketio::namespace::NamespaceCacheingLogic;
 #[cfg(all(target_os = "linux", not(feature = "development-build")))]
 use control_core::{irq_handling::set_irq_affinity, realtime::set_realtime_priority};
 use ethercat_hal::debugging::diagnosis_history::get_most_recent_diagnosis_message;
-use ethercat_hal::devices::devices_from_subdevices;
+use ethercat_hal::devices::{EthercatDevice, devices_from_subdevices};
 use ethercat_hal::devices::wago_750_354::{
     WAGO_750_354_PRODUCT_ID, WAGO_750_354_VENDOR_ID, Wago750_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::subdevice_group::{Op, PreOp, SafeOp};
+use smol::lock::RwLock;
 
 use crate::utils::{start_dnsmasq, stop_dnsmasq};
 use ethercrab::std::ethercat_now;
-use ethercrab::{MainDevice, MainDeviceConfig, PduStorage, RetryBehaviour, Timeouts};
+use ethercrab::{MainDevice, MainDeviceConfig, PduStorage, RetryBehaviour, SubDevice, SubDeviceGroup, SubDevicePdi, SubDeviceRef, Timeouts};
 use machines::machine_identification::{
-    DeviceHardwareIdentification, DeviceHardwareIdentificationEthercat, DeviceIdentification,
-    DeviceIdentificationIdentified, MachineIdentificationUnique, read_device_identifications,
+    DeviceHardwareIdentification, DeviceHardwareIdentificationEthercat, DeviceIdentification, DeviceIdentificationIdentified, MachineIdentificationUnique, machine_device_identification, read_device_identifications
 };
 use machines::registry::{MACHINE_REGISTRY, MachineRegistry};
-use machines::{Machine, MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams};
+use machines::{MACHINE_AQUAPATH_V1, Machine, MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams};
 use smol::channel::Sender;
 use socketioxide::extract::SocketRef;
 use std::{sync::Arc, time::Duration};
@@ -167,6 +168,73 @@ pub async fn set_ethercat_devices<const MAX_SUBDEVICES: usize, const MAX_PDI: us
     Ok(())
 }
 
+pub struct Groups<S = PreOp> {
+    aquapath: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, S>,
+    rest: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, S>,
+}
+
+impl Default for Groups<PreOp> {
+
+    fn default() -> Self {
+        Self {
+            aquapath: SubDeviceGroup::default(),
+            rest: SubDeviceGroup::default(),
+        }
+    }
+}
+
+impl Groups<PreOp> {
+
+    pub fn get_devices(&mut self, maindevice: &MainDevice) -> Result<Vec<Arc<RwLock<dyn EthercatDevice>>>, anyhow::Error> {
+        let mut devices = devices_from_subdevices::<MAX_SUBDEVICES, PDI_LEN>(&mut self.rest, maindevice)?;
+        devices.extend(devices_from_subdevices::<MAX_SUBDEVICES, PDI_LEN>(&mut self.aquapath, maindevice)?);
+        Ok(devices)
+    }
+
+    pub async fn into_safe_op(self, maindevice: &MainDevice<'_>) -> Result<Groups<SafeOp>, ethercrab::error::Error> {
+        Ok(Groups {
+            rest: self.rest.into_safe_op(maindevice).await?,
+            aquapath: self.aquapath.into_safe_op(maindevice).await?,
+        })
+    }
+
+    pub fn iter<'groups, 'maindevice>(&'groups self, maindevice: &'maindevice MainDevice<'groups>) -> Vec<SubDeviceRef<'maindevice, &'groups SubDevice>> {
+        self.rest.iter(&maindevice)
+            .chain(self.aquapath.iter(&maindevice))
+            .collect::<Vec<_>>()
+    }
+}
+
+impl Groups<SafeOp> {
+
+    pub async fn tx_rx_sync_system_time<'maindevice>(&self, maindevice: &'maindevice MainDevice<'maindevice>) -> Result<(), ethercrab::error::Error> {
+        self.rest.tx_rx_sync_system_time(maindevice).await?;
+        self.aquapath.tx_rx_sync_system_time(maindevice).await?;
+        Ok(())
+    }
+
+    pub async fn into_op<'maindevice>(self, maindevice: &'maindevice MainDevice<'maindevice>) -> Result<Groups<Op>, ethercrab::error::Error> {
+        Ok(Groups {
+            aquapath: self.aquapath.into_op(maindevice).await?,
+            rest: self.rest.into_op(maindevice).await?,
+        })
+    }
+}
+
+impl Groups<Op> {
+
+    pub fn iter<'groups, 'maindevice>(&'groups self, maindevice: &'maindevice MainDevice<'groups>) -> impl Iterator<Item = SubDeviceRef<'maindevice, SubDevicePdi<'maindevice, PDI_LEN>>> {
+        self.rest.iter(&maindevice)
+            .chain(self.aquapath.iter(&maindevice))
+    }
+
+    pub async fn tx_rx<'maindevice>(&self, maindevice: &'maindevice MainDevice<'maindevice>) -> Result<(), ethercrab::error::Error> {
+        self.rest.tx_rx(maindevice).await?;
+        self.aquapath.tx_rx(maindevice).await?;
+        Ok(())
+    }
+}
+
 pub async fn setup_loop(
     interface: &str,
     app_state: Arc<SharedState>,
@@ -263,12 +331,21 @@ pub async fn setup_loop(
 
     // Initalize subdevices
     // Fails if DC setup detects a mispatching working copunter, then just try again in loop
-    let mut group_preop = match maindevice
-        .init_single_group::<MAX_SUBDEVICES, PDI_LEN>(ethercat_now)
+    let mut groups_preop = match maindevice
+        .init::<MAX_SUBDEVICES, _>(ethercat_now, |groups: &Groups, subdevice| {
+            let identification = smol::block_on(machine_device_identification(subdevice, &maindevice))?;
+            let machine = identification.machine_identification_unique.machine_identification.machine;
+
+            if machine == MACHINE_AQUAPATH_V1 {
+                return Ok(&groups.aquapath);
+            }
+
+            Ok(&groups.rest)
+        })
         .await
     {
         Ok(group) => {
-            tracing::info!("Initialized {} subdevices", &group.len());
+            tracing::info!("Initialized {} subdevices", &maindevice.num_subdevices());
             group
         }
         Err(err) => Err(anyhow::anyhow!(
@@ -279,9 +356,8 @@ pub async fn setup_loop(
     };
 
     // create devices
-    let devices =
-        devices_from_subdevices::<MAX_SUBDEVICES, PDI_LEN>(&mut group_preop, &maindevice)?;
-    let subdevices = group_preop.iter(&maindevice).collect::<Vec<_>>();
+    let devices = groups_preop.get_devices(&maindevice)?;
+    let subdevices = groups_preop.iter(&maindevice);
 
     // extract device identifications
     let device_identifications = read_device_identifications(&subdevices, &maindevice)
@@ -310,7 +386,7 @@ pub async fn setup_loop(
 
     // filter devices and if Option<DeviceMachineIdentification> is Some
     // return identified_devices, identified_device_identifications, identified_subdevices
-    let (identified_device_identifications,identified_devices, identified_subdevices): (
+    let (identified_device_identifications, identified_devices, identified_subdevices): (
         Vec<_>,
         Vec<_>,
         Vec<_>,
@@ -433,10 +509,10 @@ pub async fn setup_loop(
     )
     .await?;
 
-    let group_safe = match group_preop.into_safe_op(&maindevice).await {
-        Ok(group_op) => {
+    let groups_safe = match groups_preop.into_safe_op(&maindevice).await {
+        Ok(groups) => {
             tracing::info!("Group in Safe-OP state");
-            group_op
+            groups
         }
         Err(err) => Err(anyhow::anyhow!(
             "[{}::setup_loop] Failed to put group in Safe-OP state: {:?}",
@@ -450,7 +526,7 @@ pub async fn setup_loop(
     // 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;
+            let res = groups_safe.tx_rx_sync_system_time(&maindevice).await;
             match res {
                 Ok(_) => (),
                 Err(e) => tracing::error!(
@@ -463,7 +539,7 @@ pub async fn setup_loop(
     }
 
     // Put group in operational state
-    let group_op = match group_safe.into_op(&maindevice).await {
+    let groups_op = match groups_safe.into_op(&maindevice).await {
         Ok(group_op) => {
             tracing::info!("Group in OP state");
             group_op
@@ -498,7 +574,7 @@ pub async fn setup_loop(
 
     Ok(EthercatSetup {
         devices,
-        group: group_op,
+        groups: groups_op,
         maindevice,
     })
 }

server/src/loop.rs

@@ -93,18 +93,23 @@ pub fn start_loop_thread(
                     }
                     HotThreadMessage::WriteMachineDeviceInfo(info_request) => {
                         if let Some(ethercat_setup) = &rt_loop_inputs.ethercat_setup {
-                            if let Ok(subdevice) = ethercat_setup.group.subdevice(
-                                &ethercat_setup.maindevice,
-                                info_request
-                                    .hardware_identification_ethercat
-                                    .subdevice_index,
-                            ) {
-                                let _res = smol::block_on(write_machine_device_identification(
-                                    &subdevice,
-                                    &ethercat_setup.maindevice,
-                                    &info_request.device_machine_identification,
-                                ));
-                            }
+                            // TODO: this will target the wrong subdevice. Whereever we assign the
+                            // subdevie indecies, we have to account for the groups being slip and
+                            // add an offset accordingly. For now, this is just disabled to test
+                            // stuff.
+                            //
+                            // if let Ok(subdevice) = ethercat_setup.group.subdevice(
+                            //     &ethercat_setup.maindevice,
+                            //     info_request
+                            //         .hardware_identification_ethercat
+                            //         .subdevice_index,
+                            // ) {
+                            //     let _res = smol::block_on(write_machine_device_identification(
+                            //         &subdevice,
+                            //         &ethercat_setup.maindevice,
+                            //         &info_request.device_machine_identification,
+                            //     ));
+                            // }
                         }
                     }
                     HotThreadMessage::DeleteMachine(unique_id) => {
@@ -166,13 +171,13 @@ pub async fn copy_ethercat_inputs(
     // - copy inputs to devices
     if let Some(ethercat_setup) = ethercat_setup {
         ethercat_setup
-            .group
+            .groups
             .tx_rx(&ethercat_setup.maindevice)
             .await?;
 
         // copy inputs to devices
         for (i, subdevice) in ethercat_setup
-            .group
+            .groups
             .iter(&ethercat_setup.maindevice)
             .enumerate()
         {
@@ -219,7 +224,7 @@ pub async fn copy_ethercat_outputs(
     if let Some(ethercat_setup) = ethercat_setup {
         // copy outputs from devices
         for (i, subdevice) in ethercat_setup
-            .group
+            .groups
             .iter(&ethercat_setup.maindevice)
             .enumerate()
         {