vesc_api/packet/
mod.rs

1use std::io::Error;
2
3const PACKET_MAX_LENGTH: usize = 512;
4const SMALL_PACKET_MAX_LENGTH: usize = 256;
5
6mod buffer;
7pub mod commands;
8mod crc;
9pub mod datatypes;
10
11use datatypes::McValues;
12
13pub fn get_packet<T: commands::Command>(command: T) -> Vec<u8> {
14    let data = command.get_data();
15
16    if data.len() > PACKET_MAX_LENGTH {
17        panic!("Data too long");
18    }
19
20    let mut packet = Vec::new();
21
22    if data.len() >= SMALL_PACKET_MAX_LENGTH {
23        packet.push(0x03);
24        packet.push((data.len() >> 8) as u8);
25        packet.push((data.len() & 0xFF) as u8);
26    } else {
27        packet.push(0x02);
28        packet.push(data.len() as u8);
29    }
30
31    packet.extend(data.clone());
32
33    let crc = crc::crc16(&data);
34    packet.push((crc >> 8) as u8);
35    packet.push((crc & 0xFF) as u8);
36
37    packet.push(0x03);
38    packet
39}
40
41pub enum PacketData {
42    Values(McValues),
43}
44
45pub fn process_packet(buffer: Vec<u8>) -> Result<PacketData, Error> {
46    let mut payload_length = 0;
47    let mut payload_index = 0;
48    let mut payload: Vec<u8> = Vec::new();
49
50    for (index, byte) in buffer.iter().enumerate() {
51        if *byte == 0x02 {
52            payload_length = buffer[index + 1] as usize;
53            payload_index = index + 2;
54            break;
55        }
56
57        if *byte == 0x03 {
58            payload_length = (buffer[index + 2] as usize) << 8 | buffer[index + 1] as usize;
59            payload_index = index + 3;
60            break;
61        }
62    }
63
64    if payload_length == 0 {
65        return Err(Error::new(std::io::ErrorKind::Other, "Invalid packet"));
66    }
67
68    for i in 0..payload_length {
69        payload.push(buffer[payload_index + i]);
70    }
71
72    let crc = crc::crc16(&payload);
73    let crc_received = (buffer[payload_index + payload_length] as u16) << 8
74        | buffer[payload_index + payload_length + 1] as u16;
75
76    if crc != crc_received {
77        return Err(Error::new(std::io::ErrorKind::Other, "CRC error"));
78    }
79
80    Ok(extract_data(payload))
81}
82
83fn extract_data(buffer: Vec<u8>) -> PacketData {
84    let id = buffer[0];
85    match id {
86        4 => {
87            let mut values = McValues {
88                temp_mosfet: 0.0,
89                temp_motor: 0.0,
90                current_motor: 0.0,
91                current_in: 0.0,
92                id: 0.0,
93                iq: 0.0,
94                duty_now: 0.0,
95                rpm: 0.0,
96                v_in: 0.0,
97                amp_hours: 0.0,
98                amp_hours_charged: 0.0,
99                watt_hours: 0.0,
100                watt_hours_charged: 0.0,
101                tachometer: 0,
102                tachometer_abs: 0,
103                fault_code: datatypes::McFaultCode::None,
104                pid_pos: 0.0,
105                vesc_id: 0,
106            };
107
108            //print buffer
109            println!("{:?}", buffer);
110
111            let mut index = 1;
112            values.temp_mosfet = buffer::get_float16(&buffer, 10.0, &mut index);
113            values.temp_motor = buffer::get_float16(&buffer, 10.0, &mut index);
114            values.current_motor = buffer::get_float32(&buffer, 100.0, &mut index);
115            values.current_in = buffer::get_float32(&buffer, 100.0, &mut index);
116            values.id = buffer::get_float32(&buffer, 100.0, &mut index);
117            values.iq = buffer::get_float32(&buffer, 100.0, &mut index);
118            values.duty_now = buffer::get_float16(&buffer, 1000.0, &mut index);
119            values.rpm = buffer::get_float32(&buffer, 1.0, &mut index);
120            values.v_in = buffer::get_float16(&buffer, 10.0, &mut index);
121            values.amp_hours = buffer::get_float32(&buffer, 10000.0, &mut index);
122            values.amp_hours_charged = buffer::get_float32(&buffer, 10000.0, &mut index);
123            values.watt_hours = buffer::get_float32(&buffer, 10000.0, &mut index);
124            values.watt_hours_charged = buffer::get_float32(&buffer, 10000.0, &mut index);
125            values.tachometer = buffer::get_int32(&buffer, &mut index);
126            values.tachometer_abs = buffer::get_int32(&buffer, &mut index);
127
128            PacketData::Values(values)
129        }
130        _ => panic!("Unknown packet id"),
131    }
132}
vesc_api/packet/mod.rs

vesc_api/packet/
mod.rs
