Merge branch 'mapping'

Author: jimy-byerley

examples/sdo_mapping.rs

@@ -29,10 +29,10 @@ async fn main() -> std::io::Result<()> {
     let mut slave = mapping.slave(1);
         let restatus = slave.register(SyncDirection::Read, registers::al::status);
         let mut channel = slave.channel(sdo::SyncChannel{ index: 0x1c12, direction: SyncDirection::Write, capacity: 10 });
-            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1600, 10));
+            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1600, false, 10));
                 let control = pdo.push(Sdo::<u16>::complete(0x6040));
         let mut channel = slave.channel(sdo::SyncChannel{ index: 0x1c13, direction: SyncDirection::Read, capacity: 10 });
-            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1a00, 10));
+            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1a00, false, 10));
                 let status = pdo.push(Sdo::<u16>::complete(0x6041));
                 let error = pdo.push(Sdo::<u16>::complete(0x603f));
                 let position = pdo.push(Sdo::<i32>::complete(0x6064));

examples/servodrive_run.rs

@@ -23,22 +23,22 @@ async fn main() -> std::io::Result<()> {
         std::thread::spawn(move || loop {
             master.send();
     })};
-    std::thread::sleep(std::time::Duration::from_millis(500));
+    std::thread::sleep(Duration::from_millis(500));
 
     println!("create mapping");
     let config = mapping::Config::default();
     let mapping = Mapping::new(&config);
     let mut slave = mapping.slave(1);
         let _statuscom = slave.register(SyncDirection::Read, registers::al::status);
         let mut channel = slave.channel(sdo::SyncChannel{ index: 0x1c12, direction: SyncDirection::Write, capacity: 10 });
-            let mut pdo              = channel.push(sdo::Pdo::with_capacity(0x1600, 10));
+            let mut pdo              = channel.push(sdo::Pdo::with_capacity(0x1600, false, 10));
                 let controlword      = pdo.push(sdo::cia402::controlword);
                 let target_mode      = pdo.push(sdo::cia402::target::mode);
                 let target_position  = pdo.push(sdo::cia402::target::position);
                 let _target_velocity = pdo.push(sdo::cia402::target::velocity);
                 let _target_torque = pdo.push(sdo::cia402::target::torque);
         let mut channel = slave.channel(sdo::SyncChannel{ index: 0x1c13, direction: SyncDirection::Read, capacity: 10 });
-            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1a00, 10));
+            let mut pdo = channel.push(sdo::Pdo::with_capacity(0x1a00, false, 10));
                 let statusword       = pdo.push(sdo::cia402::statusword);
                 let error            = pdo.push(sdo::cia402::error);
                 let _current_mode  = pdo.push(sdo::cia402::current::mode);

examples/slaves_discovery.rs

@@ -22,7 +22,7 @@ async fn main() -> std::io::Result<()> {
         std::thread::spawn(move || loop {
             unsafe {master.get_raw()}.send();
     })};
-    std::thread::sleep(std::time::Duration::from_millis(500));
+    std::thread::sleep(Duration::from_millis(500));
 
     master.reset_addresses().await;
 

src/mailbox.rs

@@ -177,6 +177,7 @@ impl<'b> Mailbox<'b> {
             break
         }
         // write data
+        // TODO: retry this solution with writing the last word instead of the last byte
         // we are forced to write the whole buffer (even if much bigger than data) because the slave will notice the data sent only if writing the complete buffer
         // and writing the last byte instead does not work trick it.
 //         if mailbox_size - data.len() > 32 {

src/mapping.rs

@@ -58,7 +58,7 @@ use crate::{
     rawmaster::{RawMaster, PduCommand, SlaveAddress},
     data::{PduData, Field},
     sdo::{self, Sdo, SyncDirection},
-    slave::Slave,
+    slave::{Slave, CommunicationState},
     registers,
     };
 use core::{
@@ -143,11 +143,17 @@ impl Allocator {
             master,
             allocator: self,
             allocated: slot.size,
-            config: slaves,
             offset: slot.position,
             size,
-            read: vec![0; size as usize],
-            write: vec![0; size as usize],
+            
+            config: slaves,
+            data: tokio::sync::Mutex::new(GroupData {
+                master,
+                offset: slot.position,
+                size,
+                read: vec![0; size as usize],
+                write: vec![0; size as usize],
+            }),
         }
     }
     /// check that a given mapping is compatible with the already configured slaves in the allocator
@@ -209,9 +215,15 @@ pub struct Group<'a> {
     master: &'a RawMaster,
     allocator: &'a Allocator,
     allocated: u32,
+    offset: u32,
+    size: u32,
 
     /// configuration per slave
     config: HashMap<u16, Arc<ConfigSlave>>,
+    data: tokio::sync::Mutex<GroupData<'a>>,
+}
+pub struct GroupData<'a> {
+    master: &'a RawMaster,
     /// byte offset of this data group in the logical memory
     offset: u32,
     /// byte size of this data group in the logical memory
@@ -221,14 +233,14 @@ pub struct Group<'a> {
     /// data duplex: data to write to slave
     write: Vec<u8>,
 }
-impl Group<'_> {
+impl<'a> Group<'a> {
     pub fn contains(&self, slave: u16) -> bool {
         self.config.contains_key(&slave)
     }
     /**
         write on the given slave the matching configuration from the mapping
     
-        the slave is assumed to be in state [PreOperational](crate::CommunicationState::PreOperational), and can be switched to [SafeOperational](crate::CommunicationState::SafeOperational) after this step.
+        the slave is assumed to be in state [PreOperational](CommunicationState::PreOperational), and can be switched to [SafeOperational](crate::CommunicationState::SafeOperational) after this step.
     */
     pub async fn configure(&self, slave: &Slave<'_>)  {
         let master = unsafe{ slave.raw_master() };
@@ -238,39 +250,59 @@ impl Group<'_> {
             };
         let config = &self.config[&address];
 
+        assert_eq!(slave.expected(), CommunicationState::PreOperational);
+        
         // range of physical memory to be mapped
         let physical = SLAVE_PHYSICAL_MAPPABLE;
 
         let mut coe = slave.coe().await;
+        let priority = u2::new(1);
 
         // PDO mapping
         for pdo in config.pdos.values() {
-            // pdo size must be set to zero before assigning items
-            coe.sdo_write(&pdo.config.len(), u2::new(0), 0).await;
-            for (i, sdo) in pdo.sdos.iter().enumerate() {
-                // PDO mapping
-                coe.sdo_write(&pdo.config.item(i), u2::new(0), sdo::PdoEntry::new(
-                    sdo.field.len.try_into().expect("field too big for a subitem"),
-                    sdo.sub.unwrap(),
-                    sdo.index,
-                    )).await;
+            if pdo.config.fixed {
+                // check that current sdo values are the requested ones
+                for (i, sdo) in pdo.sdos.iter().enumerate() {
+                    assert_eq!(
+                        coe.sdo_read(&pdo.config.item(i), priority).await, 
+                        sdo::PdoEntry::new(
+                            sdo.field.len.try_into().expect("field too big for a subitem"),
+                            sdo.sub.unwrap(),
+                            sdo.index,
+                            ), 
+                        "slave {} fixed pdo {}", address, pdo.config.item(i));
+                }
+            }
+            else {
+                // TODO: send as a complete SDO rather than subitems
+                // pdo size must be set to zero before assigning items
+                coe.sdo_write(&pdo.config.len(), priority, 0).await;
+                for (i, sdo) in pdo.sdos.iter().enumerate() {
+                    // PDO mapping
+                    coe.sdo_write(&pdo.config.item(i), priority, sdo::PdoEntry::new(
+                        sdo.field.len.try_into().expect("field too big for a subitem"),
+                        sdo.sub.unwrap(),
+                        sdo.index,
+                        )).await;
+                }
+                coe.sdo_write(&pdo.config.len(), priority, pdo.sdos.len() as u8).await;
             }
-            coe.sdo_write(&pdo.config.len(), u2::new(0), pdo.sdos.len() as u8).await;
         }
 
         // sync mapping
         for channel in config.channels.values() {
 
             let mut size = 0;
+            // TODO: send as a complete SDO rather than subitems
             // channel size must be set to zero before assigning items
-            coe.sdo_write(&channel.config.len(), u2::new(0), 0).await;
+            coe.sdo_write(&channel.config.len(), priority, 0).await;
             for (j, &pdo) in channel.pdos.iter().enumerate() {
-                coe.sdo_write(&channel.config.slot(j as u8), u2::new(0), pdo).await;
+                coe.sdo_write(&channel.config.slot(j as u8), priority, pdo).await;
                 size += config.pdos[&pdo].sdos.iter()
                             .map(|sdo| (sdo.field.len / 8) as u16)
                             .sum::<u16>();
             }
-            coe.sdo_write(&channel.config.len(), u2::new(0), channel.pdos.len() as u8).await;
+            coe.sdo_write(&channel.config.len(), priority, channel.pdos.len() as u8).await;
 
             // enable sync channel
             master.fpwr(address, channel.config.register(), {
@@ -307,6 +339,12 @@ impl Group<'_> {
                 }).await.one();
         }
     }
+    /// obtain exclusive access (mutex) to the data buffers
+    pub async fn data(&self) -> tokio::sync::MutexGuard<GroupData<'a>> {
+        self.data.lock().await
+    }
+}
+impl<'a> GroupData<'a> {
     /// read and write relevant data from master to segment
     pub async fn exchange(&mut self) -> &'_ mut [u8]  {
         // TODO: add a fallback implementation in case the slave does not support *RW commands
@@ -347,6 +385,8 @@ impl fmt::Debug for Group<'_> {
 }
 
 
+
+
 /// struct holding configuration informations for multiple slaves, that can be shared between multiple mappings
 #[derive(Clone, Default, Debug, Eq, PartialEq)]
 pub struct Config {

src/master.rs

@@ -2,7 +2,7 @@ use crate::{
 	socket::EthercatSocket,
 	rawmaster::{RawMaster, SlaveAddress, PduAnswer},
 	data::{PduData, Field},
-	slave::Slave,
+	slave::{Slave, CommunicationState},
 	mapping::{Allocator, Mapping, Group},
 	registers,
 	};
@@ -109,7 +109,7 @@ impl Master {
 		
 		the change will be effective on every slave on this function return, however [Slave::expect] will need to be called in order to convert salve instances to their proper state
 	*/
-	pub async fn switch(&self, target: MixedState) {
+	pub async fn switch(&self, target: CommunicationState) {
         self.raw.bwr(registers::al::control, {
 			let mut config = registers::AlControlRequest::default();
 			config.set_state(target.into());

src/rawmaster.rs

@@ -21,8 +21,11 @@ use crate::{
     };
 
 
-/// maximum frame size, currently limited to the size tolerated by its header (content size coded with 11 bits)
-const MAX_ETHERCAT_FRAME: usize = 2050;
+/// maximum frame size, currently limited to the ethernet frame size
+// size tolerated by its header (content size coded with 11 bits)
+// const MAX_ETHERCAT_FRAME: usize = 2050;
+// size tolerated by ethernet encapsulation
+const MAX_ETHERCAT_FRAME: usize = 1500;
 /// minimum PDU size
 const MIN_PDU: usize = 60;
 /// maximum number of PDU in an ethercat frame
@@ -64,8 +67,7 @@ pub struct RawMaster {
 	// synchronization signal for multitask reception
 	received: Notify,
 	sendable: Condvar,
-	send: Condvar,
-	sent: Condvar,
+	sent: Notify,
 
 	// communication state
     // states are locked using [std::sync::Mutex] since it is recommended by async-io
@@ -77,7 +79,7 @@ pub struct RawMaster {
 struct PduState {
 	last_start: usize,
 	last_end: usize,
-	last_time: Instant,
+	ready: bool,
 	/// send buffer, it contains one ethercat frame
 	send: [u8; MAX_ETHERCAT_FRAME],
 	/// reception destination, each containing a reception buffer and additional infos
@@ -99,13 +101,12 @@ impl RawMaster {
             socket: Box::new(socket),
             received: Notify::new(),
             sendable: Condvar::new(),
-            send: Condvar::new(),
-            sent: Condvar::new(),
+            sent: Notify::new(),
 
             pdu_state: Mutex::new(PduState {
                 last_start: EthercatHeader::packed_size(),
                 last_end: 0,
-                last_time: Instant::now(),
+                ready: false,
                 send: [0; MAX_ETHERCAT_FRAME],
                 receive: [0; 2*MAX_ETHERCAT_PDU].map(|_| None),
                 free: (0 .. 2*MAX_ETHERCAT_PDU).collect(),
@@ -257,17 +258,25 @@ impl RawMaster {
             let mut state = self.pdu_state.lock().unwrap();
 
             while state.free.is_empty() {
-                println!("no more token, waiting");
+                let notification = self.received.notified();
                 drop(state);
-                self.received.notified().await;
+                notification.await;
                 state = self.pdu_state.lock().unwrap();
             }
 
             // sending the buffer if necessary
-            while MAX_ETHERCAT_FRAME < state.last_end + data.len() + PduHeader::packed_size() + PduFooter::packed_size() {
-                println!("no more space, waiting");
-                self.send.notify_one();
-                state = self.sent.wait(state).unwrap();
+            while self.socket.max_frame() < state.last_end + data.len() + PduHeader::packed_size() + PduFooter::packed_size() {
+                assert!(self.socket.max_frame() > 
+                    EthercatHeader::packed_size() 
+                    + data.len() 
+                    + PduHeader::packed_size() 
+                    + PduFooter::packed_size(), "data too big for an ethercat frame");
+                state.ready = true;
+                self.sendable.notify_one();
+                let notification = self.sent.notified();
+                drop(state);
+                notification.await;
+                state = self.pdu_state.lock().unwrap();
             }
 
             // reserving a token number to ensure no other task will exchange a PDU with the same token and receive our data
@@ -292,7 +301,6 @@ impl RawMaster {
                 header.pack(place).unwrap();
             }
             else {
-                state.last_time = Instant::now();
                 state.last_end = state.last_start;
             }
 
@@ -331,14 +339,23 @@ impl RawMaster {
         };
 
         // waiting for the answer
-        while ! *ready { 
-            self.received.notified().await; 
+        loop { 
+            let notification = self.received.notified();
+            if *ready {break}
+            notification.await; 
         }
 
 		let state = self.pdu_state.lock().unwrap();
 		state.receive[token].as_ref().unwrap().answers
 	}
 
+	/// trigger sending the buffered PDUs, they will be sent as soon as possible by [Self::send] instead of waiting for the frame to be full or for the timeout
+	fn flush(&self) {
+        let mut state = self.pdu_state.lock().unwrap();
+        state.ready = true;
+        self.sendable.notify_one();
+	}
+	
 	/// extract a received frame of PDUs and buffer each for reception by an eventual `self.pdu()` future waiting for it.
 	fn pdu_receive(&self, state: &mut PduState, frame: &[u8]) {
         let mut frame = Cursor::new(frame);
@@ -390,9 +407,17 @@ impl RawMaster {
 	}
 	/// this is the socket sending handler
 	pub fn send(&self) {
-        let state = self.pdu_state.lock().unwrap();
-        let state = self.sendable.wait(state).unwrap();
-        let mut state = self.send.wait_timeout(state, self.pdu_merge_time).unwrap().0;
+        let mut state = self.pdu_state.lock().unwrap();
+        // wait indefinitely if no data to send
+        while state.last_end == 0
+            {state = self.sendable.wait(state).unwrap();}
+        // wait for more data until a timeout once data is present
+        if ! state.ready
+            {state = self.sendable.wait_timeout_while(
+                    state, 
+                    self.pdu_merge_time, 
+                    |state| ! state.ready,
+                    ).unwrap().0;}
 
         // check header
         EthercatHeader::new(
@@ -405,9 +430,10 @@ impl RawMaster {
         // we assume it is not a long operation to copy those data into the system buffer
         self.socket.send(&state.send[.. state.last_end]).unwrap();
         // reset state
+        state.ready = false;
         state.last_end = 0;
         state.last_start = EthercatHeader::packed_size();
-        drop(state);
+        self.sent.notify_waiters();
 	}
 }