telemetry_parser/ardupilot/
bin.rs

1// SPDX-License-Identifier: MIT OR Apache-2.0
2// Copyright © 2021 Adrian <adrian.eddy at gmail>
3
4use std::collections::BTreeMap;
5use std::io::*;
6use std::sync::{ Arc, atomic::AtomicBool };
7use byteorder::{ ReadBytesExt, BigEndian, LittleEndian };
8
9use crate::tags_impl::*;
10use crate::*;
11
12struct Format {
13    typ: u8,
14    _length: u8,
15    name: String,
16    format: String,
17    multipliers: Option<String>,
18    units: Option<String>,
19    labels: Vec<String>
20}
21
22#[allow(non_camel_case_types)]
23#[derive(Debug, Clone, ::serde::Serialize, ::serde::Deserialize)]
24pub enum FieldType {
25    u8(u8), i8(i8),
26    u16(u16), i16(i16),
27    u32(u32), i32(i32),
28    u64(u64), i64(i64),
29    f32(f32), f64(f64),
30    String(String),
31    Vec_i16(Vec<i16>), Vec_u16(Vec<u16>),
32    Vec_i32(Vec<i32>), Vec_u32(Vec<u32>),
33}
34
35#[derive(Debug, Clone, ::serde::Serialize, ::serde::Deserialize)]
36pub struct Field {
37    value: FieldType,
38    unit: Option<String>,
39    multiplier: Option<f64>
40}
41
42#[derive(Debug, Clone, ::serde::Serialize, ::serde::Deserialize)]
43pub struct LogItem {
44    typ: u8,
45    name: String,
46    data: BTreeMap<String, Field>
47}
48
49pub fn parse_full<T: Read + Seek, F: Fn(f64)>(stream: &mut T, size: usize, progress_cb: F, cancel_flag: Arc<AtomicBool>) -> Result<Vec<LogItem>> {
50    let mut units = BTreeMap::from([
51        ( '-', ""             .to_owned() ), // no units e.g. Pi, or a string
52        ( '?', "UNKNOWN"      .to_owned() ), // Units which haven't been worked out yet....
53        ( 'A', "A"            .to_owned() ), // Ampere
54        ( 'a', "Ah"           .to_owned() ), // Ampere hours
55        ( 'd', "deg"          .to_owned() ), // of the angular variety, -180 to 180
56        ( 'b', "B"            .to_owned() ), // bytes
57        ( 'k', "deg/s"        .to_owned() ), // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians
58        ( 'D', "deglatitude"  .to_owned() ), // degrees of latitude
59        ( 'e', "deg/s/s"      .to_owned() ), // degrees per second per second. Degrees are NOT SI, but is some situations more user-friendly than radians
60        ( 'E', "rad/s"        .to_owned() ), // radians per second
61        ( 'G', "Gauss"        .to_owned() ), // Gauss is not an SI unit, but 1 tesla = 10000 gauss so a simple replacement is not possible here
62        ( 'h', "degheading"   .to_owned() ), // 0.? to 359.?
63        ( 'i', "A.s"          .to_owned() ), // Ampere second
64        ( 'J', "W.s"          .to_owned() ), // Joule (Watt second)
65        // ( 'l', "l"         .to_owned() ), // litres
66        ( 'L', "rad/s/s"      .to_owned() ), // radians per second per second
67        ( 'm', "m"            .to_owned() ), // metres
68        ( 'n', "m/s"          .to_owned() ), // metres per second
69        // ( 'N', "N"         .to_owned() ), // Newton
70        ( 'o', "m/s/s"        .to_owned() ), // metres per second per second
71        ( 'O', "degC"         .to_owned() ), // degrees Celsius. Not SI, but Kelvin is too cumbersome for most users
72        ( '%', "%"            .to_owned() ), // percent
73        ( 'S', "satellites"   .to_owned() ), // number of satellites
74        ( 's', "s"            .to_owned() ), // seconds
75        ( 'q', "rpm"          .to_owned() ), // rounds per minute. Not SI, but sometimes more intuitive than Hertz
76        ( 'r', "rad"          .to_owned() ), // radians
77        ( 'U', "deglongitude" .to_owned() ), // degrees of longitude
78        ( 'u', "ppm"          .to_owned() ), // pulses per minute
79        ( 'v', "V"            .to_owned() ), // Volt
80        ( 'P', "Pa"           .to_owned() ), // Pascal
81        ( 'w', "Ohm"          .to_owned() ), // Ohm
82        ( 'W', "Watt"         .to_owned() ), // Watt
83        ( 'X', "W.h"          .to_owned() ), // Watt hour
84        ( 'Y', "us"           .to_owned() ), // pulse width modulation in microseconds
85        ( 'z', "Hz"           .to_owned() ), // Hertz
86        ( '#', "instance"     .to_owned() )  // (e.g.)Sensor instance number
87    ]);
88    let mut multipliers = BTreeMap::from([
89        ( '-', 0.0 ),       // no multiplier e.g. a string
90        ( '?', 1.0 ),       // multipliers which haven't been worked out yet....
91    // <leave a gap here, just in case....>
92        ( '2', 1e2 ),
93        ( '1', 1e1 ),
94        ( '0', 1e0 ),
95        ( 'A', 1e-1 ),
96        ( 'B', 1e-2 ),
97        ( 'C', 1e-3 ),
98        ( 'D', 1e-4 ),
99        ( 'E', 1e-5 ),
100        ( 'F', 1e-6 ),
101        ( 'G', 1e-7 ),
102        ( 'I', 1e-9 ),
103    // <leave a gap here, just in case....>
104        ( '!', 3.6 ), // (ampere*second => milliampere*hour) and (km/h => m/s)
105        ( '/', 3600.0 ), // (ampere*second => ampere*hour)
106    ]);
107
108    let mut stream = std::io::BufReader::with_capacity(16*1024*1024, stream);
109
110    let mut formats = BTreeMap::new();
111    let mut log = Vec::<LogItem>::new();
112
113    while (size as i64 - stream.stream_position()? as i64) >= 3 {
114        let mut update_format = BTreeMap::new();
115
116        if cancel_flag.load(std::sync::atomic::Ordering::Relaxed) { break; }
117        if size > 0 {
118            progress_cb(stream.stream_position()? as f64 / size as f64);
119        }
120
121        if stream.read_u16::<BigEndian>()? == 0xA395 {
122            let id = stream.read_u8()?;
123            if id == 0x80 { // Format message
124                let mut name = vec![0u8; 4];
125                let mut format = vec![0u8; 16];
126                let mut labels = vec![0u8; 64];
127                let typ = stream.read_u8()?;
128                let _length = stream.read_u8()?;
129                stream.read_exact(&mut name)?;
130                stream.read_exact(&mut format)?;
131                stream.read_exact(&mut labels)?;
132                formats.insert(typ, Format {
133                    typ,
134                    _length,
135                    units: None,
136                    multipliers: None,
137                    name: String::from_utf8_lossy(&name).trim_matches('\0').to_string(),
138                    format: String::from_utf8_lossy(&format).trim_matches('\0').to_string(),
139                    labels: String::from_utf8_lossy(&labels).trim_matches('\0').split(',').map(str::to_string).collect(),
140                });
141            } else if let Some(desc) = formats.get(&id) {
142                if desc.format.len() > 0 && desc.format.len() == desc.labels.len() {
143                    let mut msg = BTreeMap::new();
144                    for (i, (f, label)) in desc.format.chars().zip(&desc.labels).enumerate() {
145                        if let Err(e) = (|| -> Result<()> {
146                            let unit = desc.units.as_ref().and_then(|v| v.chars().nth(i));
147                            let unit = unit.map(|v| units.get(&v).cloned().unwrap_or_else(|| format!("{}", v)));
148                            let mult = desc.multipliers.as_ref().and_then(|v| v.chars().nth(i));
149                            let mult = mult.and_then(|v| multipliers.get(&v).copied());
150
151                            let value = match f {
152                                'a' => Some(FieldType::Vec_i16((0..32).filter_map(|_| stream.read_i16::<LittleEndian>().ok()).collect())),
153                                'b' => Some(FieldType::i8(stream.read_i8()?)),
154                                'B' => Some(FieldType::u8(stream.read_u8()?)),
155                                'h' => Some(FieldType::i16(stream.read_i16::<LittleEndian>()?)),
156                                'H' => Some(FieldType::u16(stream.read_u16::<LittleEndian>()?)),
157                                'i' => Some(FieldType::i32(stream.read_i32::<LittleEndian>()?)),
158                                'I' => Some(FieldType::u32(stream.read_u32::<LittleEndian>()?)),
159                                'f' => Some(FieldType::f32(stream.read_f32::<LittleEndian>()?)),
160                                'd' => Some(FieldType::f64(stream.read_f64::<LittleEndian>()?)),
161                                'n' | 'N' | 'Z' => {
162                                    let s = match f { 'n' => 4, 'N' => 16, 'Z' => 64, _ => 0 };
163                                    let mut data = vec![0u8; s];
164                                    stream.read_exact(&mut data)?;
165                                    Some(FieldType::String(String::from_utf8_lossy(&data).trim_matches('\0').to_string()))
166                                }
167                                'c' => Some(FieldType::Vec_i16((0..100).filter_map(|_| stream.read_i16::<LittleEndian>().ok()).collect())),
168                                'C' => Some(FieldType::Vec_u16((0..100).filter_map(|_| stream.read_u16::<LittleEndian>().ok()).collect())),
169                                'e' => Some(FieldType::Vec_i32((0..100).filter_map(|_| stream.read_i32::<LittleEndian>().ok()).collect())),
170                                'E' => Some(FieldType::Vec_u32((0..100).filter_map(|_| stream.read_u32::<LittleEndian>().ok()).collect())),
171                                'L' => Some(FieldType::i32(stream.read_i32::<LittleEndian>()?)), // latitude/longitude
172                                'M' => Some(FieldType::u8(stream.read_u8()?)), // flight mode
173                                'q' => Some(FieldType::i64(stream.read_i64::<LittleEndian>()?)),
174                                'Q' => Some(FieldType::u64(stream.read_u64::<LittleEndian>()?)),
175                                _ => {
176                                    log::error!("Invalid format {}", f);
177                                    None
178                                }
179                            };
180                            if let Some(value) = value {
181                                msg.insert(label.clone(), Field { value, unit, multiplier: mult });
182                            }
183                            Ok(())
184                        })() {
185                            log::error!("error parsing data: {e:?}")
186                        }
187                    }
188                    match desc.name.as_ref() {
189                        "UNIT" => match (msg.get("Id").map(|v| &v.value), msg.get("Label").map(|v| &v.value)) {
190                            (Some(FieldType::i8(id)), Some(FieldType::String(label))) => {
191                                units.insert(char::from(*id as u8), label.clone());
192                            },
193                            _ => { }
194                        },
195                        "MULT" => match (msg.get("Id").map(|v| &v.value), msg.get("Mult").map(|v| &v.value)) {
196                            (Some(FieldType::i8(id)), Some(FieldType::f64(mult))) => {
197                                multipliers.insert(char::from(*id as u8), *mult);
198                            },
199                            _ => { }
200                        },
201                        "FMTU" => match (msg.get("FmtType").map(|v| &v.value), msg.get("MultIds").map(|v| &v.value), msg.get("UnitIds").map(|v| &v.value)) {
202                            (Some(FieldType::u8(id)), Some(FieldType::String(mult)), Some(FieldType::String(unit))) => {
203                                update_format.insert(*id, (mult.clone(), unit.clone()));
204                            },
205                            _ => { }
206                        },
207                        _ => { }
208                    }
209                    // log::debug!("{}: {:?}", desc.name, msg);
210                    log.push(LogItem {
211                        typ: desc.typ,
212                        name: desc.name.clone(),
213                        data: msg
214                    });
215                }
216            } else {
217                log::warn!("Unknown msg: {}", id);
218            }
219        }
220        if !update_format.is_empty() {
221            for (id, (mult, unit)) in update_format {
222                if let Some(desc) = formats.get_mut(&id) {
223                    desc.units = Some(unit);
224                    desc.multipliers = Some(mult);
225                }
226            }
227        }
228    }
229    Ok(log)
230}
231
232pub fn parse<T: Read + Seek, F: Fn(f64)>(stream: &mut T, size: usize, progress_cb: F, cancel_flag: Arc<AtomicBool>) -> Result<Vec<SampleInfo>> {
233    let log = parse_full(stream, size, progress_cb, cancel_flag)?;
234
235    let mut gyro = BTreeMap::from([ ("VSTB", vec![]), ("IMU", vec![]), ("GYR", vec![]) ]);
236    let mut accl = BTreeMap::from([ ("VSTB", vec![]), ("IMU", vec![]), ("ACC", vec![]) ]);
237    let mut quats = Vec::new();
238
239    let mut first_quat_ts = None;
240
241    for l in &log {
242        if let Some(FieldType::u64(time)) = l.data.get("SampleUS").or_else(|| l.data.get("TimeUS")).map(|v| &v.value) {
243            match l.name.as_ref() {
244                "IMU" | "GYR" | "ACC" | "VSTB" => {
245                    match (l.data.get("AccX").map(|v| &v.value), l.data.get("AccY").map(|v| &v.value), l.data.get("AccZ").map(|v| &v.value)) {
246                        (Some(FieldType::f32(x)), Some(FieldType::f32(y)), Some(FieldType::f32(z))) => {
247                            accl.get_mut(l.name.as_str()).unwrap().push(TimeVector3 { t: *time as f64 / 1000000.0,
248                                x: *x as f64,
249                                y: *y as f64,
250                                z: *z as f64
251                            });
252                        },
253                        _ => { }
254                    }
255                    match (l.data.get("GyrX").map(|v| &v.value), l.data.get("GyrY").map(|v| &v.value), l.data.get("GyrZ").map(|v| &v.value)) {
256                        (Some(FieldType::f32(x)), Some(FieldType::f32(y)), Some(FieldType::f32(z))) => {
257                            gyro.get_mut(l.name.as_str()).unwrap().push(TimeVector3 { t: *time as f64 / 1000000.0,
258                                x: *x as f64,
259                                y: *y as f64,
260                                z: *z as f64
261                            });
262                        },
263                        _ => { }
264                    }
265                    match (l.data.get("Q1").map(|v| &v.value), l.data.get("Q2").map(|v| &v.value), l.data.get("Q3").map(|v| &v.value), l.data.get("Q4").map(|v| &v.value)) {
266                        (Some(FieldType::f32(w)), Some(FieldType::f32(x)), Some(FieldType::f32(y)), Some(FieldType::f32(z))) => {
267                            if first_quat_ts.is_none() {
268                                first_quat_ts = Some(*time as i64);
269                            }
270                            quats.push(TimeQuaternion {
271                                t: (*time as i64 - first_quat_ts.unwrap()) as f64 / 1000.0,
272                                v: util::multiply_quats(
273                                    (*w as f64,
274                                    *x as f64,
275                                    *y as f64,
276                                    *z as f64),
277                                    (0.5, -0.5, -0.5, 0.5),
278                                ),
279                            });
280                        },
281                        _ => { }
282                    }
283                },
284                _ => { }
285            }
286        }
287    }
288
289    // Prefer VSTB, then IMU, and then GYR/ACC. Don't add all of them because the data is duplicated then
290    let gyro = [&gyro["VSTB"], &gyro["IMU"], &gyro["GYR"]].iter().find(|v| !v.is_empty()).map(|v| v.to_vec()).unwrap_or_default();
291    let accl = [&accl["VSTB"], &accl["IMU"], &accl["ACC"]].iter().find(|v| !v.is_empty()).map(|v| v.to_vec()).unwrap_or_default();
292
293    let mut map = GroupedTagMap::new();
294
295    util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Data, "Accelerometer data", Vec_TimeVector3_f64, |v| format!("{:?}", v), accl, vec![]));
296    util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Data, "Gyroscope data",     Vec_TimeVector3_f64, |v| format!("{:?}", v), gyro, vec![]));
297    util::insert_tag(&mut map, tag!(parsed GroupId::Quaternion,    TagId::Data, "Quaternion data",    Vec_TimeQuaternion_f64, |v| format!("{:?}", v), quats, vec![]));
298
299    util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Unit, "Accelerometer unit", String, |v| v.to_string(), "m/s²".into(),  Vec::new()));
300    util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Unit, "Gyroscope unit",     String, |v| v.to_string(), "rad/s".into(), Vec::new()));
301
302    let imu_orientation = "zyx";
303    util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Orientation, "IMU orientation", String, |v| v.to_string(), imu_orientation.into(), Vec::new()));
304    util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Orientation, "IMU orientation", String, |v| v.to_string(), imu_orientation.into(), Vec::new()));
305
306    Ok(vec![
307        SampleInfo { tag_map: Some(map), ..Default::default() }
308    ])
309}
telemetry_parser/ardupilot/bin.rs

telemetry_parser/ardupilot/
bin.rs
