cands_cyphal/
lib.rs

1pub use cands_interface::{TCAN455xTranceiver, RxData, SIDConfig, XIDConfig};
2pub use cands_transport::cyphal::{CyphalMiddleware, CyphalRxFrame, CyphalRxPacketType, CRC_SIZE_BYTES};
3pub use cands_presentation::cyphal as serde;
4
5mod special_instructions;
6pub use special_instructions::digitalservo;
7
8const MTU_CAN_FD: usize = 64;
9
10#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
11const NODE_ID: u8 = 127;
12
13#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
14const SIDF1: SIDConfig = SIDConfig { sft: 3, sfec: 0, sidf1: 0x123, sidf2: 0x456 };
15
16#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
17const SIDF2: SIDConfig = SIDConfig { sft: 3, sfec: 5, sidf1: 0x123, sidf2: 0x456 };
18
19#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
20const XIDF1: XIDConfig = XIDConfig { eft: 0, efec: 0, eidf1: 0x55555, eidf2: 0x77777 };
21
22#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
23const SIDF: [SIDConfig; 2] = [SIDF1, SIDF2];
24
25#[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
26const XIDF: [XIDConfig; 1] = [XIDF1];
27
28#[cfg(any(feature="raspberrypi", feature="raspberrypi_cm"))]
29use cands_interface::GPIO_INPUT_PIN_NUM;
30
31#[cfg(all(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"), feature="drvcan_v2"))]
32const DEFAULT_TIMEOUT: std::time::Duration = std::time::Duration::from_millis(50);
33#[cfg(all(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"), feature="drvcan_v2"))]
34const DEFAULT_RETRY_COUNT: u32 = 20;
35
36
37pub struct CANInterface {
38    pub middleware: CyphalMiddleware<MTU_CAN_FD>,
39    pub driver: TCAN455xTranceiver,
40    pub rx_complete_fifo: Vec<CyphalRxFrame>,
41    pub rx_incomplete_fifo: Vec<CyphalRxFrame>,
42    #[cfg(feature="drvcan_v2")]
43    pub timeout: std::time::Duration,
44    #[cfg(feature="drvcan_v2")]
45    pub retry_count: u32,
46}
47
48
49impl CANInterface {
50    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
51    pub fn new() -> Result<Self, Box<dyn std::error::Error>> {
52        let middleware: CyphalMiddleware<MTU_CAN_FD> = CyphalMiddleware::<MTU_CAN_FD>::new(NODE_ID);
53        let driver: TCAN455xTranceiver = TCAN455xTranceiver::new()?;
54        
55        let mut interface: Self = Self {
56            middleware,
57            driver,
58            rx_complete_fifo: vec![],
59            rx_incomplete_fifo: vec![],
60            #[cfg(feature="drvcan_v2")]
61            timeout: DEFAULT_TIMEOUT,
62            #[cfg(feature="drvcan_v2")]
63            retry_count: DEFAULT_RETRY_COUNT,
64        };
65        interface.init()?;
66        
67        Ok(interface)
68    }
69
70    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
71    pub fn init(&mut self) -> Result<(), Box<dyn std::error::Error>> {
72 
73        self.driver.setup(&SIDF, &XIDF)?;
74        self.reset_rx_fifo();
75
76        // Message: dummy transfer_id to make intentional missmatch of current_transfer_id in slaves and that in this system.
77        let now: u128 = std::time::SystemTime::now()
78            .duration_since(std::time::UNIX_EPOCH).unwrap()
79            .as_millis();
80        let id_init: u8 = (now % 32) as u8;
81        self.middleware.transfer_id = id_init;
82      
83        Ok(())
84    }
85
86    #[cfg(all(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"), feature="drvcan_v2"))]
87    pub fn set_timeout(&mut self, timeout: std::time::Duration) {
88        self.timeout = timeout;
89    }
90
91    #[cfg(all(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"), feature="drvcan_v2"))]
92    pub fn set_retry_count(&mut self, retry_count: u32) {
93        self.retry_count = retry_count;
94    }
95
96    #[cfg(all(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"), feature="drvcan_v2"))]
97    pub fn reset_settings(&mut self) {
98        self.timeout = DEFAULT_TIMEOUT;
99        self.retry_count = DEFAULT_RETRY_COUNT;
100    }
101
102    #[cfg(any(feature="raspberrypi", feature="raspberrypi_cm"))]
103    pub fn gpi_read(&mut self, channel: usize) -> bool {
104        self.driver.gpi_read(channel)
105    }
106
107    #[cfg(any(feature="raspberrypi", feature="raspberrypi_cm"))]
108    pub fn gpi_read_all(&mut self) -> [bool; GPIO_INPUT_PIN_NUM] {
109        self.driver.gpi_read_all()
110    }
111
112
113    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
114    pub fn reset_rx_fifo(&mut self) {
115        self.rx_complete_fifo.clear();
116        self.rx_incomplete_fifo.clear();
117    }
118
119    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
120    pub fn send_message(&mut self, subject_id: u16, payload: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
121        match self.middleware.create_message_data(subject_id, &payload, payload.len()) {
122            Ok(packets) => {
123                for packet in packets {
124                    self.driver.transmit(packet.xid, &packet.payload, packet.payload_size)?
125                }
126            },
127            Err(err) => return Err(err)
128        }
129        Ok(())
130    }
131
132    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
133    pub fn send_response(&mut self, service_id: u16, channel: u8, payload: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
134        match self.middleware.create_response_data(channel, service_id, &payload, payload.len()) {
135            Ok(packets) => {
136                for packet in packets {
137                    self.driver.transmit(packet.xid, &packet.payload, packet.payload_size)?
138                }
139            },
140            Err(err) => return Err(err)
141        }
142        Ok(())
143    }
144
145    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
146    pub fn send_request(&mut self, service_id: u16, channel: u8, payload: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
147        match self.middleware.create_request_data(channel, service_id, &payload, payload.len()) {
148            Ok(packets) => {
149                for packet in packets {
150                    self.driver.transmit(packet.xid, &packet.payload, packet.payload_size)?
151                }
152            },
153            Err(err) => return Err(err)
154        }
155        Ok(())
156    }
157
158    /// Read received data from a FIFO buffer on a device.
159    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
160    pub fn read_device_fifo(&mut self) -> std::io::Result<Option<RxData>>{
161        match self.driver.receive() {
162            Ok(rx_data) => Ok(rx_data),
163            Err(err) => Err(err)
164        }
165    }
166
167    /// Load cyphal frames from a FIFO buffer on a user space.
168    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
169    pub fn load_frames_from_buffer(&mut self, buffer: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
170        match self.middleware.try_read(buffer) {
171            Ok(packets) => {
172                for packet in packets {
173                    match packet.status.frame_type {
174                        CyphalRxPacketType::SignleFrame => {
175                            self.rx_complete_fifo.push(CyphalRxFrame {
176                                xid: packet.xid,
177                                payload: packet.payload.to_vec(),
178                                payload_size: packet.payload_size,
179                                props: packet.props
180                            });
181                        },
182                        CyphalRxPacketType::MultiFrameStart => {
183                            self.rx_incomplete_fifo.push(CyphalRxFrame {
184                                xid: packet.xid,
185                                payload: Vec::from(&packet.payload[..packet.payload_size]),
186                                payload_size: packet.payload_size,
187                                props: packet.props
188                            });
189                        },
190                        CyphalRxPacketType::MultiFrameInProcess => {
191                            let target_frame_position: Option<usize> = self.rx_incomplete_fifo
192                                .iter()
193                                .position(|frame| (frame.xid == packet.xid) & (frame.props.port_id == packet.props.port_id));
194                            if let Some(position) = target_frame_position {
195                                self.rx_incomplete_fifo[position].payload.extend(&packet.payload[..packet.payload_size]);
196                                self.rx_incomplete_fifo[position].payload_size += packet.payload_size;
197                            }
198                        },
199                        CyphalRxPacketType::MultiFrameEnd => {
200                            let target_frame_position: Option<usize> = self.rx_incomplete_fifo
201                                .iter()
202                                .position(|frame: &CyphalRxFrame| (frame.xid == packet.xid) & (frame.props.port_id == packet.props.port_id));
203
204                            if let Some(position) = target_frame_position {
205                                self.rx_incomplete_fifo[position].payload.extend(&packet.payload[..(packet.payload_size - CRC_SIZE_BYTES as usize)]);
206                                self.rx_incomplete_fifo[position].payload_size += packet.payload_size - CRC_SIZE_BYTES as usize;
207
208                                let crc_bytes: [u8; 2] = self.rx_incomplete_fifo[position].calculate_crc()?;
209                                let crc_bytes_expected: [u8; 2] = [packet.payload[packet.payload_size - CRC_SIZE_BYTES as usize], packet.payload[packet.payload_size - CRC_SIZE_BYTES as usize + 1]];
210
211                                if crc_bytes == crc_bytes_expected {
212                                    self.rx_complete_fifo.push(self.rx_incomplete_fifo[position].clone());
213                                    self.rx_incomplete_fifo.remove(position);
214                                }
215                                else {
216                                    self.rx_incomplete_fifo.remove(position);
217                                    return Err("INVALID DATA EXIST: CRC ERROR AT MULTIFRAME CONSTRUCTION".into());
218                                }
219                            }
220                        }
221                    }
222                }
223            },
224            Err(err) => return Err(err)
225        };
226
227        Ok(())
228    }
229
230    /// Load cyphal frames from a FIFO buffer on a device.
231    /// It wraps "read_device_fifo" and "load_frames_from_buffer"
232    #[cfg(any(feature="usb-ftdi", feature="raspberrypi", feature="raspberrypi_cm"))]
233    pub fn load_frames(&mut self) -> Result<(), Box<dyn std::error::Error>> {
234        let rx_data: Option<RxData> = self.read_device_fifo()?;
235
236        if let Some(rx_data) = rx_data {
237            self.load_frames_from_buffer(&rx_data.fifo1)?
238        }
239        
240        Ok(())
241    }
242
243}
cands_cyphal/lib.rs

cands_cyphal/
lib.rs
