robojackets_robocup_rtp/
control_test_message.rs

1//!
2//! The control test message takes the data used by the robot
3//! for the current control step and sends the raw sensor values
4//! back to the base station.
5//! 
6//! This message is sent when testing the FPGA.
7//! 
8
9use ncomm_utils::packing::{Packable, PackingError};
10
11/// The size of a control update test message
12pub const CONTROL_TEST_MESSAGE_SIZE: usize = 32;
13
14#[derive(Clone, Copy, Debug, PartialEq)]
15/// A message sent back from the robot containing all relevant
16/// sensor measurements for making control decisions.
17pub struct ControlTestMessage {
18    /// The z-gyro value obtained from the IMU
19    pub gyro_z: f32,
20    /// The x-accelerometer value obtained from the IMU
21    pub accel_x: f32,
22    /// The y-accelerometer value obtained from the IMU
23    pub accel_y: f32,
24    /// The encoder velocities obtained from the FPGA
25    pub motor_encoders: [f32; 4],
26    /// The time from the last control test message to this message (us)
27    pub delta: u32,
28}
29
30impl Packable for ControlTestMessage {
31    fn len() -> usize {
32        CONTROL_TEST_MESSAGE_SIZE
33    }
34
35    fn pack(self, buffer: &mut [u8]) -> Result<(), PackingError> {
36        if buffer.len() < CONTROL_TEST_MESSAGE_SIZE {
37            return Err(PackingError::InvalidBufferSize);
38        }
39
40        buffer[0..4].copy_from_slice(&self.gyro_z.to_le_bytes());
41        buffer[4..8].copy_from_slice(&self.accel_x.to_le_bytes());
42        buffer[8..12].copy_from_slice(&self.accel_y.to_le_bytes());
43        buffer[12..16].copy_from_slice(&self.motor_encoders[0].to_le_bytes());
44        buffer[16..20].copy_from_slice(&self.motor_encoders[1].to_le_bytes());
45        buffer[20..24].copy_from_slice(&self.motor_encoders[2].to_le_bytes());
46        buffer[24..28].copy_from_slice(&self.motor_encoders[3].to_le_bytes());
47        buffer[28..32].copy_from_slice(&self.delta.to_le_bytes());
48
49        Ok(())
50    }
51
52    fn unpack(data: &[u8]) -> Result<Self, PackingError> {
53        if data.len() < CONTROL_TEST_MESSAGE_SIZE {
54            return Err(PackingError::InvalidBufferSize);
55        }
56
57        Ok(Self {
58            gyro_z: f32::from_le_bytes(data[0..4].try_into().unwrap()),
59            accel_x: f32::from_le_bytes(data[4..8].try_into().unwrap()),
60            accel_y: f32::from_le_bytes(data[8..12].try_into().unwrap()),
61            motor_encoders: [
62                f32::from_le_bytes(data[12..16].try_into().unwrap()),
63                f32::from_le_bytes(data[16..20].try_into().unwrap()),
64                f32::from_le_bytes(data[20..24].try_into().unwrap()),
65                f32::from_le_bytes(data[24..28].try_into().unwrap()),
66            ],
67            delta: u32::from_le_bytes(data[28..32].try_into().unwrap()),
68        })
69    }
70}
71
72#[cfg(test)]
73mod tests {
74    use super::*;
75
76    /// Test that Control Test Messages are packed and unpacked correctly
77    #[test]
78    fn test_control_test_message_pack_and_unpack() {
79        let message = ControlTestMessage {
80            gyro_z: 2.0,
81            accel_x: -1.5,
82            accel_y: 2.2,
83            motor_encoders: [0.7, 0.2, 0.2, 0.7],
84            delta: 100,
85        };
86
87        let mut buffer = [0u8; CONTROL_TEST_MESSAGE_SIZE];
88        message.pack(&mut buffer).unwrap();
89
90        let unpacked_message = ControlTestMessage::unpack(&buffer).unwrap();
91
92        assert_eq!(
93            message,
94            unpacked_message,
95        );
96    }
97}
robojackets_robocup_rtp/control_test_message.rs

robojackets_robocup_rtp/
control_test_message.rs
