telemetry_dataclasses/packets/
magnetometer.rs

1//! Parse magnetometer data
2
3use std::fmt;
4use log::error;
5
6use tof_dataclasses::serialization::{
7  parse_u8,
8  parse_u16,
9  parse_u16_be,
10  //parse_u32,
11  //parse_u64,
12  Serialization,
13  SerializationError,
14  //Packable
15};
16
17use crate::packets::TelemetryHeader;
18
19pub struct MagnetoMeter {
20 pub header        : TelemetryHeader,
21 pub temp          : u16, 
22 pub mag_x         : u16, 
23 pub mag_y         : u16, 
24 pub mag_z         : u16, 
25 pub acc_x         : u16, 
26 pub acc_y         : u16, 
27 pub acc_z         : u16, 
28 pub roll          : u16, 
29 pub pitch         : u16, 
30 pub yaw           : u16, 
31 pub mag_roll      : u16, 
32 pub mag_field     : u16, 
33 pub grav_field    : u16, 
34 pub expected_size : u64, // technically usize
35 pub end_byte      : u16, 
36 pub zero          : u8, 
37 pub ndata         : u8, 
38}
39
40impl MagnetoMeter {
41  pub fn new() -> Self {
42    Self {
43     header            : TelemetryHeader::new(),
44     temp              : 0, 
45     mag_x             : 0, 
46     mag_y             : 0, 
47     mag_z             : 0, 
48     acc_x             : 0, 
49     acc_y             : 0, 
50     acc_z             : 0, 
51     roll              : 0, 
52     pitch             : 0, 
53     yaw               : 0, 
54     mag_roll          : 0, 
55     mag_field         : 0, 
56     grav_field        : 0, 
57     expected_size     : 0, // technically usize
58     end_byte          : 0, 
59     zero              : 0, 
60     ndata             : 0, 
61    }
62  }
63}
64
65impl Default for MagnetoMeter {
66  fn default() -> Self {
67    Self::new()
68  }
69}
70
71impl fmt::Display for MagnetoMeter {
72  fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
73    let mut repr = String::from("<MagnetoMeter: ");
74    repr += &(format!("\n {}", self.header));
75    repr += &(format!("\n temp          : {}", self.temp            ));   
76    repr += &(format!("\n mag_x         : {}", self.mag_x           ));   
77    repr += &(format!("\n mag_y         : {}", self.mag_y           ));   
78    repr += &(format!("\n mag_z         : {}", self.mag_z           ));   
79    repr += &(format!("\n acc_x         : {}", self.acc_x           ));   
80    repr += &(format!("\n acc_y         : {}", self.acc_y           ));   
81    repr += &(format!("\n acc_z         : {}", self.acc_z           ));   
82    repr += &(format!("\n roll          : {}", self.roll            ));   
83    repr += &(format!("\n pitch         : {}", self.pitch           ));   
84    repr += &(format!("\n yaw           : {}", self.yaw             ));   
85    repr += &(format!("\n mag_roll      : {}", self.mag_roll        ));   
86    repr += &(format!("\n mag_field     : {}", self.mag_field       ));   
87    repr += &(format!("\n grav_field    : {}", self.grav_field      ));   
88    repr += &(format!("\n expected_size : {}", self.expected_size   ));   
89    repr += &(format!("\n end_byte      : {}", self.end_byte        ));   
90    repr += &(format!("\n zero          : {}", self.zero            ));   
91    repr += &(format!("\n ndata         : {}", self.ndata           ));   
92    write!(f, "{}", repr)
93  }
94}
95
96impl Serialization for MagnetoMeter {
97  
98  const HEAD : u16   = 0x90eb;
99  const TAIL : u16   = 0x0000; // there is no tail for telemetry packets
100  const SIZE : usize = 57; 
101  
102  fn from_bytestream(stream : &Vec<u8>,
103                     pos    : &mut usize)
104    -> Result<Self, SerializationError> {
105    let mut mag = Self::new();
106    if stream.len() < Self::SIZE {
107      error!("We got {} bytes, but need {}!", stream.len(),Self::SIZE);
108      return Err(SerializationError::StreamTooShort);
109    }
110    mag.header  = TelemetryHeader::from_bytestream(stream, pos)?;
111    // we do have to deal with a bunch of empty bytes
112    *pos += 1;
113    let mut n_data = parse_u8(stream, pos);
114    if n_data != 16 {
115      error!("Decoding of magnetometer packet faILed! We expected 16 data bytes, but got {} instead!", n_data);
116      return Err(SerializationError::WrongByteSize);
117    }
118    //*pos += n_empty as usize;
119    mag.mag_x = parse_u16_be(stream, pos);
120    mag.acc_x = parse_u16_be(stream, pos);
121    mag.mag_y = parse_u16_be(stream, pos);
122    mag.acc_y = parse_u16_be(stream, pos);
123    mag.mag_z = parse_u16_be(stream, pos);
124    mag.acc_z = parse_u16_be(stream, pos);
125    mag.temp  = parse_u16(stream, pos);
126    //i += from_bytes(&bytes[i],temp);
127    //i +=2; // the other temp we do not understand
128    *pos += 2; // ALEX - "the other temp we do not understand"
129    //i += from_bytes(&bytes[i],zero);
130    mag.zero  = parse_u8(stream, pos);
131    if mag.zero != 0 {
132      // FIXME - better error type
133      error!("Decoding of magnetometer packet failed! Byte whcih should be zero is not zero!");
134      return Err(SerializationError::WrongByteSize);
135    }
136    *pos += 1; // ALEX - "the checksum we are not checking"
137    mag.end_byte = parse_u16_be(stream, pos);
138    if mag.end_byte != 32767 {
139      error!("Decoding of magnetormeter packet faailed! Tail incorrect!");
140      return Err(SerializationError::TailInvalid);
141    }
142    *pos += 1; // empty bytes that we do not care about from the first magnetometer packet
143    n_data     = parse_u8(stream, pos);
144    if n_data != 16 {
145      error!("The second magnetometer data chunk seems to have the wrong size! ({} instead of 16)", n_data);
146      return Err(SerializationError::WrongByteSize);
147    };
148    mag.roll        = parse_u16_be(stream, pos); 
149    mag.mag_roll    = parse_u16_be(stream, pos); 
150    mag.pitch       = parse_u16_be(stream, pos); 
151    mag.mag_field   = parse_u16_be(stream, pos); 
152    mag.yaw         = parse_u16_be(stream, pos); 
153    mag.grav_field  = parse_u16_be(stream, pos); 
154    *pos += 4; // ALEX - "more temp data we are not reading out"
155    mag.zero = parse_u8(stream, pos);
156    if mag.zero != 0 {
157      // FIXME - better error type
158      error!("Decoding of magnetometer packet failed! Byte whcih should be zero is not zero!");
159      return Err(SerializationError::WrongByteSize);
160    }
161    *pos += 1; // ALEX  - "another checksum (from second packet) we are not checking"
162    mag.end_byte = parse_u16_be(stream, pos);
163    if mag.end_byte != 32767 {
164      error!("Decoding of magnetormeter packet faailed! Tail incorrect!");
165      return Err(SerializationError::TailInvalid);
166    }
167    Ok(mag)
168  }
169}
telemetry_dataclasses/packets/magnetometer.rs

telemetry_dataclasses/packets/
magnetometer.rs
