agent_sim/can/
dbc.rs

1use crate::sim::types::{CAN_FLAG_EXTENDED, CAN_FLAG_FD, SimCanFrame};
2use can_dbc::{ByteOrder, Dbc, MessageId, MultiplexIndicator, ValueType};
3use std::collections::HashMap;
4use std::path::Path;
5
6#[derive(Debug, Clone)]
7pub struct DbcSignalDef {
8    pub name: String,
9    pub frame_key: u32,
10    pub arb_id: u32,
11    pub extended: bool,
12    pub message_size: u8,
13    pub start_bit: u64,
14    pub size: u64,
15    pub byte_order: ByteOrder,
16    pub value_type: ValueType,
17    pub factor: f64,
18    pub offset: f64,
19    pub min: f64,
20    pub max: f64,
21    pub unit: Option<String>,
22}
23
24#[derive(Debug, Clone)]
25pub struct DbcBusOverlay {
26    signals_by_name: HashMap<String, DbcSignalDef>,
27}
28
29impl DbcBusOverlay {
30    pub fn load(path: &Path) -> Result<Self, String> {
31        let content = std::fs::read_to_string(path)
32            .map_err(|e| format!("failed to read DBC '{}': {e}", path.display()))?;
33        let dbc = Dbc::try_from(content.as_str())
34            .map_err(|e| format!("failed to parse DBC '{}': {e}", path.display()))?;
35        let mut signals_by_name = HashMap::new();
36
37        for message in dbc.messages {
38            let (arb_id, extended) = match message.id {
39                MessageId::Standard(id) => (u32::from(id), false),
40                MessageId::Extended(id) => (id, true),
41            };
42            let message_size = u8::try_from(message.size).map_err(|_| {
43                format!(
44                    "DBC message '{}' has invalid size {}; max supported is 255 bytes",
45                    message.name, message.size
46                )
47            })?;
48            if message_size > 64 {
49                return Err(format!(
50                    "DBC message '{}' size {} exceeds CAN FD max payload (64)",
51                    message.name, message_size
52                ));
53            }
54
55            for signal in message.signals {
56                if signal.multiplexer_indicator != MultiplexIndicator::Plain {
57                    return Err(format!(
58                        "DBC signal '{}.{}' uses multiplexing, which is unsupported in this version",
59                        message.name, signal.name
60                    ));
61                }
62                let size = signal.size;
63                if size == 0 || size > 64 {
64                    return Err(format!(
65                        "DBC signal '{}.{}' has invalid size {}",
66                        message.name, signal.name, size
67                    ));
68                }
69                let frame_key = frame_key(arb_id, extended);
70                signals_by_name.insert(
71                    signal.name.clone(),
72                    DbcSignalDef {
73                        name: signal.name,
74                        frame_key,
75                        arb_id,
76                        extended,
77                        message_size,
78                        start_bit: signal.start_bit,
79                        size,
80                        byte_order: signal.byte_order,
81                        value_type: signal.value_type,
82                        factor: signal.factor,
83                        offset: signal.offset,
84                        min: signal.min,
85                        max: signal.max,
86                        unit: if signal.unit.is_empty() {
87                            None
88                        } else {
89                            Some(signal.unit)
90                        },
91                    },
92                );
93            }
94        }
95
96        Ok(Self { signals_by_name })
97    }
98
99    pub fn signal(&self, name: &str) -> Option<&DbcSignalDef> {
100        self.signals_by_name.get(name)
101    }
102
103    pub fn signal_names(&self) -> impl Iterator<Item = &String> {
104        self.signals_by_name.keys()
105    }
106}
107
108pub fn decode_signal(frame: &SimCanFrame, signal: &DbcSignalDef) -> Result<f64, String> {
109    let raw = extract_raw(frame, signal)?;
110    let numeric = match signal.value_type {
111        ValueType::Signed => signed_from_raw(raw, signal.size) as f64,
112        ValueType::Unsigned => raw as f64,
113    };
114    Ok(numeric * signal.factor + signal.offset)
115}
116
117pub fn encode_signal(
118    frame: &mut SimCanFrame,
119    signal: &DbcSignalDef,
120    value: f64,
121) -> Result<(), String> {
122    if !value.is_finite() {
123        return Err(format!(
124            "invalid value for CAN signal '{}': {value}",
125            signal.name
126        ));
127    }
128    let has_explicit_range = signal.min != 0.0 || signal.max != 0.0;
129    if has_explicit_range && (value < signal.min || value > signal.max) {
130        return Err(format!(
131            "value {value} is out of DBC range [{}, {}] for CAN signal '{}'",
132            signal.min, signal.max, signal.name
133        ));
134    }
135    if signal.factor == 0.0 {
136        return Err(format!(
137            "DBC signal '{}' has zero factor, cannot encode",
138            signal.name
139        ));
140    }
141
142    let raw_float = (value - signal.offset) / signal.factor;
143    let raw_i64 = raw_float.round() as i64;
144    let raw_u64 = match signal.value_type {
145        ValueType::Signed => {
146            let min = -(1_i128 << (signal.size - 1));
147            let max = (1_i128 << (signal.size - 1)) - 1;
148            let val = i128::from(raw_i64);
149            if val < min || val > max {
150                return Err(format!(
151                    "encoded raw value {raw_i64} exceeds signed {}-bit range for signal '{}'",
152                    signal.size, signal.name
153                ));
154            }
155            let mask = if signal.size == 64 {
156                u64::MAX
157            } else {
158                (1_u64 << signal.size) - 1
159            };
160            (raw_i64 as i128 as u64) & mask
161        }
162        ValueType::Unsigned => {
163            if raw_i64 < 0 {
164                return Err(format!(
165                    "encoded raw value {raw_i64} is negative for unsigned signal '{}'",
166                    signal.name
167                ));
168            }
169            let max = if signal.size == 64 {
170                u64::MAX
171            } else {
172                (1_u64 << signal.size) - 1
173            };
174            let raw = raw_i64 as u64;
175            if raw > max {
176                return Err(format!(
177                    "encoded raw value {raw} exceeds unsigned {}-bit range for signal '{}'",
178                    signal.size, signal.name
179                ));
180            }
181            raw
182        }
183    };
184
185    insert_raw(frame, signal, raw_u64)?;
186    if signal.message_size > frame.len {
187        frame.len = signal.message_size;
188    }
189    if frame.len > 8 {
190        frame.flags |= CAN_FLAG_FD;
191    } else {
192        frame.flags &= !CAN_FLAG_FD;
193    }
194    if signal.extended {
195        frame.flags |= CAN_FLAG_EXTENDED;
196    } else {
197        frame.flags &= !CAN_FLAG_EXTENDED;
198    }
199    Ok(())
200}
201
202pub fn frame_key_from_frame(frame: &SimCanFrame) -> u32 {
203    frame_key(frame.arb_id, (frame.flags & CAN_FLAG_EXTENDED) != 0)
204}
205
206fn frame_key(arb_id: u32, extended: bool) -> u32 {
207    if extended { arb_id | (1 << 31) } else { arb_id }
208}
209
210fn extract_raw(frame: &SimCanFrame, signal: &DbcSignalDef) -> Result<u64, String> {
211    if signal.size > 64 {
212        return Err(format!(
213            "signal '{}' size {} exceeds 64 bits",
214            signal.name, signal.size
215        ));
216    }
217    match signal.byte_order {
218        ByteOrder::LittleEndian => {
219            let mut raw = 0_u64;
220            for idx in 0..signal.size {
221                let frame_bit = signal.start_bit + idx;
222                let bit = get_bit(&frame.data, frame_bit)?;
223                raw |= u64::from(bit) << idx;
224            }
225            Ok(raw)
226        }
227        ByteOrder::BigEndian => {
228            let mut raw = 0_u64;
229            let mut bit_pos = signal.start_bit as i64;
230            for _ in 0..signal.size {
231                let bit = get_bit(&frame.data, bit_pos as u64)?;
232                raw = (raw << 1) | u64::from(bit);
233                bit_pos = next_motorola_bit(bit_pos);
234            }
235            Ok(raw)
236        }
237    }
238}
239
240fn insert_raw(frame: &mut SimCanFrame, signal: &DbcSignalDef, raw: u64) -> Result<(), String> {
241    match signal.byte_order {
242        ByteOrder::LittleEndian => {
243            for idx in 0..signal.size {
244                let frame_bit = signal.start_bit + idx;
245                let bit = ((raw >> idx) & 1) as u8;
246                set_bit(&mut frame.data, frame_bit, bit)?;
247            }
248        }
249        ByteOrder::BigEndian => {
250            let mut bit_pos = signal.start_bit as i64;
251            for idx in 0..signal.size {
252                let shift = signal.size - 1 - idx;
253                let bit = ((raw >> shift) & 1) as u8;
254                set_bit(&mut frame.data, bit_pos as u64, bit)?;
255                bit_pos = next_motorola_bit(bit_pos);
256            }
257        }
258    }
259    Ok(())
260}
261
262fn get_bit(data: &[u8; 64], index: u64) -> Result<u8, String> {
263    let byte_index = usize::try_from(index / 8).map_err(|_| "bit index overflow".to_string())?;
264    if byte_index >= data.len() {
265        return Err(format!("bit index {index} is out of bounds"));
266    }
267    let bit_index = (index % 8) as u8;
268    Ok((data[byte_index] >> bit_index) & 1)
269}
270
271fn set_bit(data: &mut [u8; 64], index: u64, bit: u8) -> Result<(), String> {
272    let byte_index = usize::try_from(index / 8).map_err(|_| "bit index overflow".to_string())?;
273    if byte_index >= data.len() {
274        return Err(format!("bit index {index} is out of bounds"));
275    }
276    let mask = 1_u8 << (index % 8);
277    if bit == 0 {
278        data[byte_index] &= !mask;
279    } else {
280        data[byte_index] |= mask;
281    }
282    Ok(())
283}
284
285fn next_motorola_bit(current: i64) -> i64 {
286    if current % 8 == 0 {
287        current + 15
288    } else {
289        current - 1
290    }
291}
292
293fn signed_from_raw(raw: u64, bits: u64) -> i64 {
294    if bits == 0 {
295        return 0;
296    }
297    if bits >= 64 {
298        return raw as i64;
299    }
300    let sign_bit = 1_u64 << (bits - 1);
301    if (raw & sign_bit) == 0 {
302        raw as i64
303    } else {
304        (raw as i64) - ((1_u64 << bits) as i64)
305    }
306}
307
308#[cfg(test)]
309mod tests {
310    use super::*;
311
312    fn sample_signal(min: f64, max: f64) -> DbcSignalDef {
313        DbcSignalDef {
314            name: "EngineRPM".to_string(),
315            frame_key: 0x123,
316            arb_id: 0x123,
317            extended: false,
318            message_size: 8,
319            start_bit: 0,
320            size: 16,
321            byte_order: ByteOrder::LittleEndian,
322            value_type: ValueType::Unsigned,
323            factor: 0.25,
324            offset: 0.0,
325            min,
326            max,
327            unit: Some("RPM".to_string()),
328        }
329    }
330
331    fn empty_frame() -> SimCanFrame {
332        SimCanFrame {
333            arb_id: 0x123,
334            len: 0,
335            flags: 0,
336            data: [0; 64],
337        }
338    }
339
340    #[test]
341    fn encode_signal_allows_nonzero_with_unspecified_dbc_range() {
342        let signal = sample_signal(0.0, 0.0);
343        let mut frame = empty_frame();
344        encode_signal(&mut frame, &signal, 1500.0)
345            .expect("value should encode when DBC range is unspecified [0|0]");
346        let decoded = decode_signal(&frame, &signal).expect("encoded signal should decode");
347        assert!((decoded - 1500.0).abs() < f64::EPSILON);
348    }
349
350    #[test]
351    fn encode_signal_rejects_values_outside_explicit_dbc_range() {
352        let signal = sample_signal(0.0, 250.0);
353        let mut frame = empty_frame();
354        let err = encode_signal(&mut frame, &signal, 1500.0).expect_err("value should be rejected");
355        assert!(err.contains("out of DBC range [0, 250]"));
356    }
357}
agent_sim/can/dbc.rs

agent_sim/can/
dbc.rs
