kbot_pwrbrd/
lib.rs

1use socketcan::{CanSocket, Socket, CanFrame, ExtendedId, EmbeddedFrame, CanError, Id};
2use std::error::Error;
3use std::time::Duration;
4use serde::{Serialize, Deserialize};
5pub struct PowerBoard {
6    socket: CanSocket,
7}
8
9
10/// Builds a 29-bit arbitration ID from:
11///  - `target_address` (lowest 8 bits)
12///  - `comm_type` (next 13 bits)
13///  - `reserved` (top 8 bits)
14fn arbitration_id(target_address: u8, comm_type: u16, reserved: u8) -> Option<ExtendedId> {
15    // bits 0..7   -> target_address
16    // bits 8..20  -> comm_type
17    // bits 21..28 -> reserved
18    let arbitration_id = (target_address as u32 & 0xFF)
19        | ((comm_type as u32 & 0x1FFF) << 8)
20        | ((reserved as u32 & 0xFF) << 21);
21
22    ExtendedId::new(arbitration_id)
23}
24
25/// 0x1003: Status Frame structure.
26/// Mirrors the Python logic:
27///  - battery_voltage (bytes 0..1)
28///  - motor_voltage   (bytes 2..3)
29///  - current         (bytes 4..5)
30///  - fault_raw       (bytes 6..7), plus bitfield flags
31#[derive(Debug, Clone, Serialize, Deserialize)]
32pub struct PowerBoardGeneralFrame {
33    pub battery_voltage: f32,
34    pub motor_voltage: f32,
35    pub current: f32,
36    pub fault_raw: u16,
37    pub power_chip_oc: bool,
38    pub power_chip_ot: bool,
39    pub power_chip_sc: bool,
40    pub sampling_oc: bool,
41    pub vbus_ov: bool,
42    pub vbus_uv: bool,
43    pub vmbus_ov: bool,
44    pub vmbus_uv: bool,
45    pub raw_data: Vec<u8>,
46}
47
48/// 0x1004: Status Frame structure.
49///  - left_leg_power  (bytes 0..1)
50///  - right_leg_power (bytes 2..3)
51///  - left_arm_power  (bytes 4..5)
52///  - right_arm_power (bytes 6..7)
53#[derive(Debug, Clone, Serialize, Deserialize)]
54pub struct PowerBoardLimbFrame {
55    pub left_leg_power: f32,
56    pub right_leg_power: f32,
57    pub left_arm_power: f32,
58    pub right_arm_power: f32,
59    pub raw_data: Vec<u8>,
60}
61
62/// An enum to represent "parsed" power board frames.
63#[derive(Debug, Clone, Serialize, Deserialize)]
64pub enum PowerBoardFrame {
65    General(PowerBoardGeneralFrame), // 0x1003
66    Limbs(PowerBoardLimbFrame),     // 0x1004
67    Unknown(u32, Vec<u8>),
68}
69
70#[derive(Debug, Clone, Serialize, Deserialize)]
71pub struct ControlCommand {
72    pub fan: bool,
73    pub pre_charge: bool,
74    pub motor: bool,
75    pub main: bool,
76    pub restart: bool,
77    pub clear_fault: bool,
78    pub auto_report: bool,
79    pub reserved: bool,
80}
81
82impl PowerBoard {
83    pub fn new(interface: &str) -> Result<Self, Box<dyn Error>> {
84        let socket = CanSocket::open(interface)?;
85        socket.set_read_timeout(Duration::from_millis(100))?;
86        Ok(Self { socket })
87    }
88
89    /// Configures the board to enable auto-reporting and normal operation.
90    pub fn configure_board(&self) -> Result<(), Box<dyn Error>> {
91        let command = ControlCommand {
92            fan: false,
93            pre_charge: true,
94            motor: true,
95            main: true,
96            restart: false,
97            clear_fault: false,
98            auto_report: true,
99            reserved: false,
100        };
101        self.send_control_frame(&command)?;
102        Ok(())
103    }
104
105    pub fn configure_and_clear_faults(&self) -> Result<(), Box<dyn Error>> {
106        let command = ControlCommand {
107            fan: false,
108            pre_charge: true,
109            motor: true,
110            main: true,
111            restart: false,
112            clear_fault: true,
113            auto_report: true,
114            reserved: false,
115        };
116        self.send_control_frame(&command)?;
117        Ok(())
118    }
119
120    /// Sends a 0x1001 Control Frame to enable auto-reporting on the power board.
121    /// 
122    /// Byte layout (0..7):
123    ///     Byte0: 1/0 -> Fan
124    ///     Byte1: 1/0 -> Pre-charge
125    ///     Byte2: 1/0 -> Motor output
126    ///     Byte3: 1/0 -> Main control
127    ///     Byte4: 1/0 -> Restart
128    ///     Byte5: 1/0 -> Clear fault
129    ///     Byte6: 1/0 -> Auto-report data (100ms)
130    ///     Byte7: 1/0 -> Reserved
131    pub fn send_control_frame(&self, command: &ControlCommand) -> Result<(), Box<dyn Error>> {
132        let target_address = 0xAA; // Should be 0xAA for all power boards
133        let comm_type = 0x1001; // Control code
134        let reserved = 0x00;
135
136        // Build the 29-bit extended ID
137        let id = arbitration_id(target_address, comm_type, reserved)
138            .ok_or("Invalid 29-bit ID for control frame")?;
139
140        let data = [command.fan as u8, command.pre_charge as u8, command.motor as u8, command.main as u8, command.restart as u8, command.clear_fault as u8, command.auto_report as u8, command.reserved as u8];
141
142        // Construct the extended CAN frame
143        let frame = CanFrame::new(id, &data)
144            .ok_or("Failed to create 0x1001 control CAN frame")?;
145
146        // Send
147        self.socket.write_frame(&frame)?;
148        tracing::info!("Sent 0x1001 Control Frame with command: {:?}", command);
149        Ok(())
150    }
151
152    /// Parse a single incoming CAN frame as a power board message.
153    /// Returns `None` if the frame is not extended or data is invalid.
154    pub fn parse_power_board_message(frame: &CanFrame) -> Option<PowerBoardFrame> {
155        // Must be an extended ID
156        let id = frame.id();
157        if !matches!(id, Id::Extended(_)) {
158            // Not a 29-bit ID
159            return None;
160        }
161
162        // Extract the raw 29-bit arbitration ID
163        let arbitration_id = match id {
164            Id::Extended(ext_id) => ext_id.as_raw(),
165            _ => return None,
166        };
167
168        // Bits [0..7]:   target_address
169        // Bits [8..20]:  comm_type
170        // Bits [21..28]: reserved
171        let target_address = (arbitration_id & 0xFF) as u8;
172        let comm_type = ((arbitration_id >> 8) & 0x1FFF) as u16;
173        let _reserved = ((arbitration_id >> 21) & 0xFF) as u8;
174
175        let data = frame.data();
176        if data.len() < 8 {
177            // Not enough data to parse
178            return None;
179        }
180
181        // Dispatch based on comm_type
182        match comm_type {
183            0x1003 => {
184                // parse 0x1003 frame
185                Some(PowerBoardFrame::General(parse_status_frame_1003(data)))
186            }
187            0x1004 => {
188                // parse 0x1004 frame
189                Some(PowerBoardFrame::Limbs(parse_limb_power_frame(data)))
190            }
191            _ => {
192                // Unknown or not implemented
193                Some(PowerBoardFrame::Unknown(arbitration_id, data.to_vec()))
194            }
195        }
196    }
197
198    /// Example of reading a single frame from the CAN socket and parsing it.
199    pub fn read_frame(&self) -> Result<Option<PowerBoardFrame>, Box<dyn Error>> {
200        let frame = match self.socket.read_frame() {
201            Ok(f) => f,
202            Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => {
203                // Timed out (no frame within read_timeout)
204                return Ok(None);
205            }
206            Err(e) => return Err(Box::new(e)),
207        };
208        Ok(Self::parse_power_board_message(&frame))
209    }
210}
211
212/// Parse the 0x1003 data payload into `PowerBoardGeneralFrame`.
213fn parse_status_frame_1003(data: &[u8]) -> PowerBoardGeneralFrame {
214    // Byte0-1: Battery Voltage (big-endian, /100)
215    let battery_voltage_raw = ((data[0] as u16) << 8) | (data[1] as u16);
216    let battery_voltage = battery_voltage_raw as f32 / 100.0;
217
218    // Byte2-3: Motor Voltage (big-endian, /100)
219    let motor_voltage_raw = ((data[2] as u16) << 8) | (data[3] as u16);
220    let motor_voltage = motor_voltage_raw as f32 / 100.0;
221
222    // Byte4-5: Current (big-endian, /100)
223    let current_raw = ((data[4] as u16) << 8) | (data[5] as u16);
224    let current = current_raw as f32 / 100.0;
225
226    // Byte6-7: Fault Status (bitfield)
227    let fault_raw = ((data[6] as u16) << 8) | (data[7] as u16);
228
229    PowerBoardGeneralFrame {
230        battery_voltage,
231        motor_voltage,
232        current,
233        fault_raw,
234        power_chip_oc: (fault_raw & (1 << 0)) != 0,
235        power_chip_ot: (fault_raw & (1 << 1)) != 0,
236        power_chip_sc: (fault_raw & (1 << 2)) != 0,
237        sampling_oc:   (fault_raw & (1 << 3)) != 0,
238        vbus_ov:       (fault_raw & (1 << 4)) != 0,
239        vbus_uv:       (fault_raw & (1 << 5)) != 0,
240        vmbus_ov:      (fault_raw & (1 << 6)) != 0,
241        vmbus_uv:      (fault_raw & (1 << 7)) != 0,
242        raw_data: data.to_vec(),
243    }
244}
245
246/// Parse the 0x1004 data payload into `PowerBoardLimbFrame`.
247fn parse_limb_power_frame(data: &[u8]) -> PowerBoardLimbFrame {
248    let left_leg_raw = ((data[0] as u16) << 8) | (data[1] as u16);
249    let left_leg_power = left_leg_raw as f32 / 100.0;
250
251    let right_leg_raw = ((data[2] as u16) << 8) | (data[3] as u16);
252    let right_leg_power = right_leg_raw as f32 / 100.0;
253
254    let left_arm_raw = ((data[4] as u16) << 8) | (data[5] as u16);
255    let left_arm_power = left_arm_raw as f32 / 100.0;
256
257    let right_arm_raw = ((data[6] as u16) << 8) | (data[7] as u16);
258    let right_arm_power = right_arm_raw as f32 / 100.0;
259
260    PowerBoardLimbFrame {
261        left_leg_power,
262        right_leg_power,
263        left_arm_power,
264        right_arm_power,
265        raw_data: data.to_vec(),
266    }
267}
kbot_pwrbrd/lib.rs

kbot_pwrbrd/
lib.rs
