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

//! SBET file format.

use std::fmt::Debug;
use std::fs::File;
use std::io::{BufReader, Read};
use std::iter::IntoIterator;
use std::path::Path;

use byteorder;
use byteorder::{LittleEndian, ReadBytesExt};

use {Error, Result};
use point::Point;
use source::Source;
use units::Radians;

/// 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>>> {
        Ok(Reader { reader: BufReader::new(try!(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>> {
        let time = match self.reader.read_f64::<LittleEndian>() {
            Ok(time) => time,
            Err(byteorder::Error::UnexpectedEOF) => return Ok(None),
            Err(err) => return Err(Error::from(err)),
        };
        Ok(Some(Point {
            time: time,
            latitude: Radians(try!(self.reader.read_f64::<LittleEndian>())),
            longitude: Radians(try!(self.reader.read_f64::<LittleEndian>())),
            altitude: try!(self.reader.read_f64::<LittleEndian>()),
            x_velocity: Some(try!(self.reader.read_f64::<LittleEndian>())),
            y_velocity: Some(try!(self.reader.read_f64::<LittleEndian>())),
            z_velocity: Some(try!(self.reader.read_f64::<LittleEndian>())),
            roll: Radians(try!(self.reader.read_f64::<LittleEndian>())),
            pitch: Radians(try!(self.reader.read_f64::<LittleEndian>())),
            yaw: Radians(try!(self.reader.read_f64::<LittleEndian>())),
            wander_angle: Some(Radians(try!(self.reader.read_f64::<LittleEndian>()))),
            x_acceleration: Some(try!(self.reader.read_f64::<LittleEndian>())),
            y_acceleration: Some(try!(self.reader.read_f64::<LittleEndian>())),
            z_acceleration: Some(try!(self.reader.read_f64::<LittleEndian>())),
            x_angular_rate: Some(Radians(try!(self.reader.read_f64::<LittleEndian>()))),
            y_angular_rate: Some(Radians(try!(self.reader.read_f64::<LittleEndian>()))),
            z_angular_rate: Some(Radians(try!(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>> {
        self.read_point()
    }
}

#[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);
    }
}