pos 0.2.0

Reads GNSS/IMU position and accuracy files.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131

//! SBET file format.

use crate::point::Point;
use crate::source::Source;
use crate::units::Radians;
use crate::Error;
use byteorder::{LittleEndian, ReadBytesExt};
use std::fmt::Debug;
use std::fs::File;
use std::io::{BufReader, Read};
use std::iter::IntoIterator;
use std::path::Path;

/// An SBET reader.
#[derive(Debug)]
pub struct Reader<R: Read> {
    reader: R,
}

impl Reader<BufReader<File>> {
    /// Opens a reader for a path.
    ///
    /// # Examples
    ///
    /// ```
    /// use pos::sbet::Reader;
    /// let reader = Reader::from_path("data/2-points.sbet").unwrap();
    /// ```
    pub fn from_path<P: AsRef<Path>>(path: P) -> Result<Reader<BufReader<File>>, std::io::Error> {
        Ok(Reader {
            reader: BufReader::new(File::open(path)?),
        })
    }
}

impl<R: Read> Reader<R> {
    /// Reads a point from this reader.
    ///
    /// Returns none if the file is at its end when this reader starts reading. We have to do it
    /// this way since sbet files don't have a point count.
    ///
    /// # Examples
    ///
    /// ```
    /// use pos::sbet::Reader;
    /// let mut reader = Reader::from_path("data/2-points.sbet").unwrap();
    /// let point = reader.read_point().unwrap().unwrap();
    /// ```
    pub fn read_point(&mut self) -> Result<Option<Point>, std::io::Error> {
        use std::io::ErrorKind;

        let time = match self.reader.read_f64::<LittleEndian>() {
            Ok(time) => time,
            Err(err) => match err.kind() {
                ErrorKind::UnexpectedEof => return Ok(None),
                _ => return Err(err.into()),
            },
        };
        Ok(Some(Point {
            time,
            latitude: Radians(self.reader.read_f64::<LittleEndian>()?),
            longitude: Radians(self.reader.read_f64::<LittleEndian>()?),
            altitude: self.reader.read_f64::<LittleEndian>()?,
            x_velocity: Some(self.reader.read_f64::<LittleEndian>()?),
            y_velocity: Some(self.reader.read_f64::<LittleEndian>()?),
            z_velocity: Some(self.reader.read_f64::<LittleEndian>()?),
            roll: Radians(self.reader.read_f64::<LittleEndian>()?),
            pitch: Radians(self.reader.read_f64::<LittleEndian>()?),
            yaw: Radians(self.reader.read_f64::<LittleEndian>()?),
            wander_angle: Some(Radians(self.reader.read_f64::<LittleEndian>()?)),
            x_acceleration: Some(self.reader.read_f64::<LittleEndian>()?),
            y_acceleration: Some(self.reader.read_f64::<LittleEndian>()?),
            z_acceleration: Some(self.reader.read_f64::<LittleEndian>()?),
            x_angular_rate: Some(Radians(self.reader.read_f64::<LittleEndian>()?)),
            y_angular_rate: Some(Radians(self.reader.read_f64::<LittleEndian>()?)),
            z_angular_rate: Some(Radians(self.reader.read_f64::<LittleEndian>()?)),
            ..Default::default()
        }))
    }
}

impl<R: Read> IntoIterator for Reader<R> {
    type Item = Point;
    type IntoIter = ReaderIterator<R>;
    fn into_iter(self) -> Self::IntoIter {
        ReaderIterator { reader: self }
    }
}

/// An iterator over an sbet reader.
#[derive(Debug)]
pub struct ReaderIterator<R: Read> {
    reader: Reader<R>,
}

impl<R: Read> Iterator for ReaderIterator<R> {
    type Item = Point;
    fn next(&mut self) -> Option<Self::Item> {
        self.reader.read_point().unwrap()
    }
}

impl<R: Debug + Read> Source for Reader<R> {
    fn source(&mut self) -> Result<Option<Point>, Error> {
        self.read_point().map_err(Error::from)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn read_file() {
        let reader = Reader::from_path("data/2-points.sbet").unwrap();
        let points: Vec<_> = reader.into_iter().collect();
        assert_eq!(2, points.len());
        let point = points[0];
        assert!((1.5163100e5 - point.time).abs() < 1e-2, "{}", point.time);
        assert!(
            (0.5680211 - point.latitude.0).abs() < 1e-7,
            "{:?}",
            point.latitude
        );
        assert!(
            (1.5163110e5 - points[1].time).abs() < 1e-1,
            "{}",
            points[1].time
        );
    }
}