telemetry_parser/
tags_impl.rs

1// SPDX-License-Identifier: MIT OR Apache-2.0
2// Copyright © 2021 Adrian <adrian.eddy at gmail>
3
4use once_cell::unsync::OnceCell;
5use serde::Serialize;
6use std::collections::*;
7
8macro_rules! declare_groups {
9    ($($field:ident),*,) => {
10        #[allow(dead_code)]
11        #[derive(PartialEq, Eq, PartialOrd, Ord, Hash, Clone, Debug)]
12        pub enum GroupId {
13            $($field,)*
14            UnknownGroup(u32),
15            Custom(String),
16            Any // For filtering, shouldn't be used directly
17        }
18        impl Serialize for GroupId {
19            fn serialize<S>(&self, s: S) -> Result<S::Ok, S::Error> where S: serde::Serializer {
20                match self {
21                    GroupId::UnknownGroup(x) => s.serialize_str(&format!("0x{:x}", x)),
22                    GroupId::Custom(x)       => s.serialize_str(x),
23                    GroupId::Any             => s.serialize_str("*"),
24                    $(GroupId::$field        => s.serialize_str(stringify!($field)),)*
25                }
26            }
27        }
28        impl std::fmt::Display for GroupId {
29            fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
30                match self {
31                    GroupId::UnknownGroup(x) => f.write_str(&format!("0x{:x}", x)),
32                    GroupId::Custom(x)       => f.write_str(x),
33                    GroupId::Any             => f.write_str("*"),
34                    $(GroupId::$field        => f.write_str(stringify!($field)),)*
35                }
36            }
37        }
38        impl std::str::FromStr for GroupId {
39            type Err = std::num::ParseIntError;
40
41            fn from_str(s: &str) -> Result<Self, Self::Err> {
42                Ok(match s {
43                    $(stringify!($field) => GroupId::$field,)*
44                    "*" => GroupId::Any,
45                    _ if s.starts_with("0x") => GroupId::UnknownGroup(u32::from_str_radix(&s[2..], 16)?),
46                    _ => GroupId::Custom(s.to_string())
47                })
48            }
49        }
50    }
51}
52
53macro_rules! declare_ids {
54    ($($field:ident),*,) => {
55        #[allow(dead_code)]
56        #[derive(PartialEq, Eq, PartialOrd, Ord, Hash, Clone, Debug)]
57        pub enum TagId {
58            $($field,)*
59            Unknown(u32),
60            File(String),
61            Custom(String),
62            Any // For filtering, shouldn't be used directly
63        }
64        impl Serialize for TagId {
65            fn serialize<S>(&self, s: S) -> Result<S::Ok, S::Error> where S: serde::Serializer {
66                match self {
67                    TagId::Unknown(x)      => s.serialize_str(&format!("0x{:x}", x)),
68                    TagId::Custom(x)       => s.serialize_str(x),
69                    TagId::File(x)         => s.serialize_str(x),
70                    TagId::Any             => s.serialize_str("*"),
71                    $(TagId::$field        => s.serialize_str(stringify!($field)),)*
72                }
73            }
74        }
75        impl std::fmt::Display for TagId {
76            fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
77                match self {
78                    TagId::Unknown(x)      => f.write_str(&format!("0x{:x}", x)),
79                    TagId::Custom(x)       => f.write_str(x),
80                    TagId::File(x)         => f.write_str(x),
81                    TagId::Any             => f.write_str("*"),
82                    $(TagId::$field        => f.write_str(stringify!($field)),)*
83                }
84            }
85        }
86        impl std::str::FromStr for TagId {
87            type Err = std::num::ParseIntError;
88
89            fn from_str(s: &str) -> Result<Self, Self::Err> {
90                Ok(match s {
91                    $(stringify!($field) => TagId::$field,)*
92                    "*" => TagId::Any,
93                    _ if s.starts_with("0x") => TagId::Unknown(s.parse::<u32>()?),
94                    _ => TagId::Custom(s.to_string())
95                })
96            }
97        }
98    }
99}
100
101macro_rules! declare_types {
102    ($($field:ident:$type:ty),*,) => {
103        #[allow(non_camel_case_types)]
104        #[allow(dead_code)]
105        #[derive(Clone)]
106        pub enum TagValue {
107            $($field(ValueType<$type>),)*
108            Unknown(ValueType<()>),
109        }
110        impl ToString for TagValue {
111            fn to_string(&self) -> String {
112                match &self {
113                    $(TagValue::$field(t) => (t.format_fn)(t.get()),)*
114                    TagValue::Unknown(t) => format!("{} bytes: {}", t.raw_data.len(), crate::util::to_hex(&t.raw_data[..])),
115                }
116            }
117        }
118        impl Serialize for TagValue {
119            fn serialize<S>(&self, s: S) -> Result<S::Ok, S::Error> where S: serde::Serializer {
120                match &self {
121                    $(TagValue::$field(t) => serde::Serialize::serialize(t.get(), s),)*
122                    TagValue::Unknown(t) => s.serialize_bytes(&t.raw_data),
123                }
124            }
125        }
126
127        /*impl<T> std::convert::TryInto<ValueType<T>> for TagValue {
128            type Error = &'static str;
129            fn try_into(self) -> Result<ValueType<T>, Self::Error> {
130                match self {
131                    $(TagValue::$field(t) => Ok(t),)*
132                    TagValue::Unknown(t) => Err("Unknown TagValue"),
133                    _ => Err("Unknown TagValue")
134                }
135            }
136        }*/
137        pub trait GetWithType<T> { fn get_t(&self, k: TagId) -> Option<&T>; }
138        $(
139            impl std::convert::TryInto<$type> for TagValue {
140                type Error = &'static str;
141                fn try_into(self) -> Result<$type, Self::Error> {
142                    if let TagValue::$field(t) = self {
143                        return Ok(t.get().clone());
144                    }
145                    Err("Unknown TagValue")
146                }
147            }
148            impl GetWithType<$type> for TagMap {
149                fn get_t(&self, k: TagId) -> Option<&$type> {
150                    if let Some(v) = self.get(&k) {
151                        if let TagValue::$field(vv) = &v.value {
152                            return Some(vv.get());
153                        }
154                    }
155                    None
156                }
157            }
158        )*
159        impl std::fmt::Debug for TagValue {
160            fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
161                match &self {
162                    $(TagValue::$field(t) => f.write_fmt(format_args!("TagValue(\n\tType: {}\n\tValue: {:?}\n\tFormatted value: {}\n)", stringify!($field), &t.get(), self.to_string())),)*
163                    TagValue::Unknown(_)  => f.write_fmt(format_args!("TagValue(\n\tType: Unknown\n\tValue: {}\n)", self.to_string()))
164                }
165            }
166        }
167    };
168}
169
170include!("tags.rs");
171
172#[derive(Debug, Clone)]
173pub struct TagDescription {
174    pub group: GroupId,
175    pub id: TagId,
176    pub native_id: Option<u32>,
177    pub description: String,
178    pub value: TagValue,
179}
180
181type ParseFn<T> = fn(&mut std::io::Cursor::<&[u8]>) -> std::io::Result<T>;
182
183#[derive(Clone)]
184pub struct ValueType<T> {
185    parse_fn: Option<ParseFn<T>>,
186    format_fn: fn(&T) -> String,
187    parsed_value: OnceCell<T>,
188    pub raw_data: Vec<u8>
189}
190impl<T: Default> ValueType<T> {
191    pub fn new(parse_fn: ParseFn<T>, format_fn: fn(&T) -> String, raw_data: Vec<u8>) -> ValueType<T> {
192        ValueType {
193            parse_fn: Some(parse_fn),
194            format_fn,
195            raw_data,
196            parsed_value: once_cell::unsync::OnceCell::new()
197        }
198    }
199    pub fn new_parsed(format_fn: fn(&T) -> String, parsed_value: T, raw_data: Vec<u8>) -> ValueType<T> {
200        let v = once_cell::unsync::OnceCell::new();
201        let _ = v.set(parsed_value);
202        ValueType {
203            parse_fn: None,
204            format_fn,
205            raw_data,
206            parsed_value: v
207        }
208    }
209    pub fn get(&self) -> &T {
210        self.parsed_value.get_or_init(|| {
211            let mut tag_slice = std::io::Cursor::new(&self.raw_data[..]);
212            match (self.parse_fn.expect("value not parsed"))(&mut tag_slice) {
213                Ok(v) => { return v; },
214                Err(e) => {
215                    log::error!("Parsing error {:?}: {}", e, pretty_hex::pretty_hex(&self.raw_data));
216                    return T::default();
217                }
218            }
219        })
220    }
221    pub fn get_mut(&mut self) -> &mut T {
222        self.parsed_value.get_mut().unwrap()
223    }
224}
225
226#[derive(Debug, Clone, Serialize, Default)]
227pub struct Vector3<T> {
228    pub x: T,
229    pub y: T,
230    pub z: T,
231}
232impl<T: std::convert::Into<f64>> Vector3<T> {
233    pub fn into_scaled(self, raw2unit: &f64, unit2deg: &f64) -> Vector3<f64> {
234        Vector3 {
235            x: self.x.into() / raw2unit * unit2deg,
236            y: self.y.into() / raw2unit * unit2deg,
237            z: self.z.into() / raw2unit * unit2deg,
238        }
239    }
240}
241impl Vector3<f64> {
242    pub fn orient(&self, io: &[u8]) -> Vector3<f64> {
243        let map = |o: u8| -> f64 {
244            match o as char {
245                'X' => self.x, 'x' => -self.x,
246                'Y' => self.y, 'y' => -self.y,
247                'Z' => self.z, 'z' => -self.z,
248                err => { panic!("Invalid orientation {}", err); }
249            }
250        };
251        Vector3 { x: map(io[0]), y: map(io[1]), z: map(io[2]) }
252    }
253}
254#[derive(Debug, Clone, Serialize, Default)]
255pub struct TimeVector3<T, TT = T> {
256    pub t: TT,
257    pub x: T,
258    pub y: T,
259    pub z: T,
260}
261impl<T: std::convert::Into<f64>> TimeVector3<T> {
262    pub fn into_scaled(self, raw2unit: &f64, unit2deg: &f64) -> Vector3<f64> {
263        Vector3 {
264            x: self.x.into() / raw2unit * unit2deg,
265            y: self.y.into() / raw2unit * unit2deg,
266            z: self.z.into() / raw2unit * unit2deg,
267        }
268    }
269}
270#[derive(Debug, Clone, Serialize, Default)]
271pub struct TimeArray8<T> {
272    pub t: f64,
273    pub v: [T; 8]
274}
275#[derive(Debug, Clone, Serialize, Default)]
276pub struct TimeArray4<T> {
277    pub t: f64,
278    pub v: [T; 4]
279}
280#[derive(Debug, Clone, Serialize, Default)]
281pub struct TimeArray2<T> {
282    pub t: f64,
283    pub v: [T; 2]
284}
285#[derive(Debug, Clone, Serialize, Default)]
286pub struct TimeScalar<T> {
287    pub t: f64,
288    pub v: T
289}
290#[derive(Debug, Clone, Serialize, Default)]
291pub struct Quaternion<T> {
292    pub w: T,
293    pub x: T,
294    pub y: T,
295    pub z: T,
296}
297impl<T: Copy + std::ops::Mul<Output = T> + std::ops::Sub<Output = T> + std::ops::Add<Output = T>> std::ops::Mul for Quaternion<T> {
298    type Output = Self;
299    fn mul(self, rhs: Self) -> Self {
300        Self {
301            w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
302            x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
303            y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
304            z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w
305        }
306    }
307}
308#[derive(Debug, Clone, Serialize, Default)]
309pub struct TimeQuaternion<T> {
310    pub t: f64,
311    pub v: Quaternion<T>
312}
313
314#[derive(Debug, Clone, Serialize, Default)]
315pub struct GpsData {
316    pub is_acquired: bool,
317    pub unix_timestamp: f64,
318    pub lat: f64,
319    pub lon: f64,
320    pub speed: f64, // in km/h
321    pub track: f64,
322    pub altitude: f64, // in m
323}
324
325#[macro_export]
326macro_rules! tag {
327    ($group:expr, $id:expr, $name:expr, $type:ident, $format:literal, $body:expr, $tag_data:expr) => {
328        TagDescription { group: $group, id: $id, description: $name.to_owned(), value: TagValue::$type(ValueType::new($body, |v| format!($format, v), $tag_data.to_vec())), native_id: None }
329    };
330    ($group:expr, $id:expr, $name:expr, $type:ident, $format:expr, $body:expr, $tag_data:expr) => {
331        TagDescription { group: $group, id: $id, description: $name.to_owned(), value: TagValue::$type(ValueType::new($body, $format, $tag_data.to_vec())), native_id: None }
332    };
333    (parsed $group:expr, $id:expr, $name:expr, $type:ident, $format:expr, $val:expr, $tag_data:expr) => {
334        TagDescription { group: $group, id: $id, description: $name.to_owned(), value: TagValue::$type(ValueType::new_parsed($format, $val, $tag_data.to_vec())), native_id: None }
335    };
336    ($group:expr, $id:expr, $name:expr, $tag_data:expr) => {
337        TagDescription { group: $group, id: $id, description: $name.to_owned(), value: TagValue::Unknown(ValueType::new(|_| Ok(()), |_| "".into(), $tag_data.to_vec())), native_id: None }
338    };
339}
340
341pub type TagMap = BTreeMap<TagId, TagDescription>;
342pub type GroupedTagMap = BTreeMap<GroupId, TagMap>;
telemetry_parser/tags_impl.rs

telemetry_parser/
tags_impl.rs
