telemetry_parser/gyroflow/
mod.rs

1// SPDX-License-Identifier: MIT OR Apache-2.0
2// Copyright © 2021 Elvin
3
4use std::collections::BTreeMap;
5use std::io::*;
6use std::sync::{ Arc, atomic::AtomicBool };
7use std::path::{ Path, PathBuf };
8
9use crate::tags_impl::*;
10use crate::*;
11
12#[derive(Default)]
13pub struct Gyroflow {
14    pub model: Option<String>,
15    pub gyro_path: Option<PathBuf>,
16    vendor: String,
17    frame_readout_time: Option<f64>
18}
19
20// .gcsv format as described here: https://docs.gyroflow.xyz/logging/gcsv/
21
22impl Gyroflow {
23    pub fn detect<P: AsRef<Path>>(buffer: &[u8], filepath: P) -> Option<Self> {
24        let filename = filepath.as_ref().file_name().map(|x| x.to_string_lossy()).unwrap_or_default();
25
26        let mut gyro_path = None;
27        if !filename.to_ascii_lowercase().ends_with(".gcsv") && filepath.as_ref().with_extension("gcsv").exists() {
28            gyro_path = Some(filepath.as_ref().with_extension("gcsv").into());
29        }
30        let gyro_buf = if let Some(gyro) = &gyro_path {
31            std::fs::read(gyro).ok()?
32        } else {
33            buffer.to_vec()
34        };
35
36        let match_hdr = |line: &[u8]| -> bool {
37            &gyro_buf[0..line.len().min(gyro_buf.len())] == line
38        };
39        if match_hdr(b"GYROFLOW IMU LOG") || match_hdr(b"CAMERA IMU LOG"){
40            let mut header = BTreeMap::new();
41
42            // get header block
43            let header_block = &gyro_buf[0..gyro_buf.len().min(500)];
44
45            let mut csv = csv::ReaderBuilder::new()
46                .has_headers(false)
47                .flexible(true)
48                .trim(csv::Trim::All)
49                .from_reader(Cursor::new(header_block));
50
51            for row in csv.records() {
52                let row = row.ok()?;
53                if row.len() == 2 {
54                    header.insert(row[0].to_owned(), row[1].to_owned());
55                    continue;
56                }
57                if &row[0] == "t" { break; }
58            }
59
60            // let version = header.remove("version").unwrap_or("1.0".to_owned());
61            let id = header.remove("id").unwrap_or("NoID".to_owned()).replace("_", " ");
62            let vendor = header.remove("vendor").unwrap_or("gcsv".to_owned());
63            let frame_readout_time = if header.contains_key("frame_readout_time") {
64                // assume top down if not given
65                let readout_direction = header.remove("frame_readout_direction").unwrap_or("0".to_owned());
66                let readout_time = header.remove("frame_readout_time").unwrap_or("0.0".to_owned()).parse::<f64>().unwrap_or_default();
67                match readout_direction.as_str() {
68                    "0" => Some(readout_time), // top -> bottom
69                    "1" => Some(-readout_time), // bottom -> top
70                    "2" => None, // left/right not supported
71                    "3" => None,
72                    _ => None
73                }
74            } else { None };
75
76            let model = Some(id);
77            return Some(Self { model, gyro_path, vendor, frame_readout_time });
78        }
79        None
80    }
81
82    pub fn parse<T: Read + Seek, F: Fn(f64)>(&mut self, stream: &mut T, _size: usize, _progress_cb: F, _cancel_flag: Arc<AtomicBool>) -> Result<Vec<SampleInfo>> {
83        let e = |_| -> Error { ErrorKind::InvalidData.into() };
84
85        let gyro_buf = if let Some(gyro) = &self.gyro_path {
86            std::fs::read(gyro)?
87        } else {
88            let mut vec = Vec::new();
89            stream.read_to_end(&mut vec)?;
90            vec
91        };
92
93        let mut header = BTreeMap::new();
94
95        let mut gyro = Vec::new();
96        let mut accl = Vec::new();
97        let mut magn = Vec::new();
98
99        let mut csv = csv::ReaderBuilder::new()
100            .has_headers(false)
101            .flexible(true)
102            .trim(csv::Trim::All)
103            .from_reader(Cursor::new(gyro_buf));
104
105        let mut passed_header = false;
106
107        let mut time_scale = 0.001; // default to millisecond
108
109        for row in csv.records() {
110            let row = row?;
111
112            if row.len() == 1 {
113                continue; // first line
114            } else if row.len() == 2 && !passed_header {
115                header.insert(row[0].to_owned(), row[1].to_owned());
116                continue;
117            } else if &row[0] == "t" || &row[0] == "time" {
118                passed_header = true;
119                time_scale =  header.remove("tscale").unwrap_or("0.001".to_owned()).parse::<f64>().unwrap();
120                continue;
121            }
122
123            let time = row[0].parse::<f64>().map_err(e)? * time_scale;
124            if row.len() >= 4 {
125                gyro.push(TimeVector3 {
126                    t: time,
127                    x: row[1].parse::<f64>().unwrap_or_default(),
128                    y: row[2].parse::<f64>().unwrap_or_default(),
129                    z: row[3].parse::<f64>().unwrap_or_default(),
130                });
131            }
132            if row.len() >= 7 {
133                accl.push(TimeVector3 {
134                    t: time,
135                    x: row[4].parse::<f64>().unwrap_or_default(),
136                    y: row[5].parse::<f64>().unwrap_or_default(),
137                    z: row[6].parse::<f64>().unwrap_or_default()
138                });
139            }
140            if row.len() >= 10 {
141                magn.push(TimeVector3 {
142                    t: time,
143                    x: row[7].parse::<f64>().unwrap_or_default(),
144                    y: row[8].parse::<f64>().unwrap_or_default(),
145                    z: row[9].parse::<f64>().unwrap_or_default()
146                });
147            }
148        }
149        let accl_scale = 1.0 / header.remove("ascale").unwrap_or("1.0".to_owned()).parse::<f64>().unwrap();
150        let gyro_scale = 1.0 / header.remove("gscale").unwrap_or("1.0".to_owned()).parse::<f64>().unwrap() * std::f64::consts::PI / 180.0;
151        let mag_scale = 100.0 / header.remove("mscale").unwrap_or("1.0".to_owned()).parse::<f64>().unwrap(); // Gauss to microtesla
152        let imu_orientation = header.remove("orientation").unwrap_or("xzY".to_owned()); // default
153
154        let mut map = GroupedTagMap::new();
155
156        if let Some(lensprofile) = header.remove("lensprofile") {
157            util::insert_tag(&mut map, tag!(parsed GroupId::Lens, TagId::Name, "Lens profile", String, |v| v.to_string(), lensprofile, Vec::new()));
158        }
159
160        util::insert_tag(&mut map,
161            tag!(parsed GroupId::Default, TagId::Metadata, "Extra metadata", Json, |v| format!("{:?}", v), serde_json::to_value(header).map_err(|_| Error::new(ErrorKind::Other, "Serialize error"))?, vec![])
162        );
163
164        util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Data, "Accelerometer data", Vec_TimeVector3_f64, |v| format!("{:?}", v), accl, vec![]));
165        util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Data, "Gyroscope data",     Vec_TimeVector3_f64, |v| format!("{:?}", v), gyro, vec![]));
166        util::insert_tag(&mut map, tag!(parsed GroupId::Magnetometer,  TagId::Data, "Magnetometer data",  Vec_TimeVector3_f64, |v| format!("{:?}", v), magn, vec![]));
167
168        util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Unit, "Accelerometer unit", String, |v| v.to_string(), "g".into(),  Vec::new()));
169        util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Unit, "Gyroscope unit",     String, |v| v.to_string(), "deg/s".into(), Vec::new()));
170        util::insert_tag(&mut map, tag!(parsed GroupId::Magnetometer,  TagId::Unit, "Magnetometer unit",  String, |v| v.to_string(), "μT".into(), Vec::new()));
171
172        util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Scale, "Gyroscope scale",     f64, |v| format!("{:?}", v), gyro_scale, vec![]));
173        util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Scale, "Accelerometer scale", f64, |v| format!("{:?}", v), accl_scale, vec![]));
174        util::insert_tag(&mut map, tag!(parsed GroupId::Magnetometer,  TagId::Scale, "Magnetometer scale",  f64, |v| format!("{:?}", v), mag_scale, vec![]));
175
176        util::insert_tag(&mut map, tag!(parsed GroupId::Gyroscope,     TagId::Orientation, "IMU orientation", String, |v| v.to_string(), imu_orientation.to_string(), Vec::new()));
177        util::insert_tag(&mut map, tag!(parsed GroupId::Accelerometer, TagId::Orientation, "IMU orientation", String, |v| v.to_string(), imu_orientation.to_string(), Vec::new()));
178        util::insert_tag(&mut map, tag!(parsed GroupId::Magnetometer,  TagId::Orientation, "IMU orientation", String, |v| v.to_string(), imu_orientation.to_string(), Vec::new()));
179
180        Ok(vec![
181            SampleInfo { tag_map: Some(map), ..Default::default() }
182        ])
183    }
184
185    pub fn normalize_imu_orientation(v: String) -> String {
186        v
187    }
188
189    pub fn camera_type(&self) -> String {
190        self.vendor.to_owned()
191    }
192
193    pub fn frame_readout_time(&self) -> Option<f64> {
194        self.frame_readout_time
195    }
196}
telemetry_parser/gyroflow/mod.rs

telemetry_parser/gyroflow/
mod.rs
