rustodrive/
canframe.rs

1use crate::state::{self, ReadComm};
2use crate::state::ODriveCommand;
3use socketcan::CANFrame;
4
5pub type CANRequest = ODriveCANFrame;
6pub type CANResponse = ODriveCANFrame;
7
8//CAN Ticket is a CAN command factory ready to be sent to the axis
9pub fn ticket(id: usize, command: ODriveCommand, data: [u8; 8]) -> CANRequest {
10    CANRequest { axis: id as u32, cmd: command, data: data }
11}
12
13pub fn read_can(id: usize, command: ReadComm) -> CANRequest {
14    ticket(id, ODriveCommand::Read(command), [0; 8])
15}
16
17#[derive(Clone, Copy, PartialEq, Eq, Debug, Hash)]
18pub struct ODriveCANFrame {
19    pub axis: u32,
20    pub cmd: ODriveCommand,
21    pub data: [u8; 8],
22}
23
24impl ODriveCANFrame {
25    const AXIS_BITS: u32 = 5;
26
27    pub fn to_can(&self, rtr: bool) -> socketcan::CANFrame {
28        let id = self.axis << Self::AXIS_BITS | self.get_cmd_id();
29        return socketcan::CANFrame::new(id, &self.data, rtr, false).unwrap(); // ODrive rquires the RTR bitset be enabled for call/response
30    }
31
32    fn get_cmd_id(&self) -> u32 {
33        match self.cmd {
34            ODriveCommand::Read(cmd) => cmd as u32,
35            ODriveCommand::Write(cmd) => cmd as u32,
36        }
37    }
38
39    fn to_cmd(can_id: u32) -> ODriveCommand {
40        // 0x1F is 00011111 in binary. Take the last 5 bits to get the command
41        let cmd_id = can_id & 0x1F;
42
43        // Then try converting to a write command
44        match TryInto::<state::WriteComm>::try_into(cmd_id) {
45            Ok(cmd) => return ODriveCommand::Write(cmd),
46            Err(_) => {}
47        }
48
49        // Try first converting to a read command
50        match TryInto::<state::ReadComm>::try_into(cmd_id) {
51            Ok(cmd) => return ODriveCommand::Read(cmd),
52            Err(_) => panic!("CAN ID {} not able to be converted to a command", can_id),
53        }
54    }
55
56    pub fn from_can(frame: &CANFrame) -> Self {
57        // Get the first 5 bits
58        let axis = frame.id() >> Self::AXIS_BITS;
59        let cmd = Self::to_cmd(frame.id());
60
61        ODriveCANFrame {
62            axis,
63            cmd,
64            data: frame.data().try_into().expect(
65                "Error with CANFrame. data() returned slice that could not be coerced into [u8; 8]",
66            ),
67        }
68    }
69
70    // If the command and axis IDs match, then it must be the response
71    pub fn is_response(&self, other: &ODriveCANFrame) -> bool {
72        self.axis == other.axis && self.cmd == other.cmd
73    }
74}
75
76/// This is a wrapper struct to keep track of which thread sent what ODriveCANFrame
77#[derive(Clone, PartialEq, Debug)]
78pub struct ThreadCANFrame {
79    pub thread_name: &'static str,
80    pub body: ODriveCANFrame,
81}
82
83
84#[cfg(test)]
85mod tests {
86    use crate::{
87        state::{ODriveCommand, ReadComm, WriteComm},
88        canframe::{CANRequest, CANResponse},
89    };
90
91    use super::ODriveCANFrame;
92
93    #[test]
94    fn test_conversion_to_frame() {
95        // Tests if converting from the CAN frame and back retains the right data
96        let msg1 = CANRequest {
97            axis: 0x1,
98            cmd: ODriveCommand::Write(WriteComm::SetInputPosition), // this is cmd id 0x0C
99            data: [0; 8],
100        };
101
102        let msg2 = CANRequest {
103            axis: 0x293874,
104            cmd: ODriveCommand::Read(ReadComm::GetEncoderCount), // this is cmd id 0x0C
105            data: [0; 8],
106        };
107
108        // Calculated by hand. See this example https://docs.odriverobotics.com/v/latest/can-protocol.html#can-frame
109        let rtr_enabled = true;
110        let can_frame = msg1.to_can(rtr_enabled);
111        assert_eq!(can_frame.id(), 0x2C);
112        assert_eq!(can_frame.data(), [0, 0, 0, 0, 0, 0, 0, 0]);
113        assert_eq!(can_frame.is_rtr(), rtr_enabled);
114
115        // Test it converts back properly
116        assert_eq!(msg1, msg1.clone()); // test that equals works
117        assert_eq!(msg1, ODriveCANFrame::from_can(&can_frame));
118
119        // test that conversion works with read messages
120        // Converting from CANFrame to ODriveCANFrame ignores
121        //the rtr bit so either true/false is fine
122        assert_eq!(msg2, ODriveCANFrame::from_can(&msg2.to_can(true)));
123    }
124
125    #[test]
126    #[should_panic]
127    fn test_command_not_found() {
128        ODriveCANFrame::to_cmd(0xFFFF);
129    }
130
131    #[test]
132    fn test_is_response() {
133        let msg1 = CANRequest {
134            axis: 0x1,
135            cmd: ODriveCommand::Write(WriteComm::SetInputPosition), // this is cmd id 0x0C
136            data: [0; 8],
137        };
138        let fake_response = CANResponse {
139            axis: 0x1,
140            cmd: ODriveCommand::Write(WriteComm::SetInputPosition), // this is cmd id 0x0C
141            data: [1; 8], // the data has changed but the rest is the same
142        };
143        assert_eq!(msg1.is_response(&fake_response), true);
144    }
145}
rustodrive/canframe.rs

rustodrive/
canframe.rs
