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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169

//! Position and orientation quality files.

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

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

use {Error, Result};
use io::read_full;
use point::{Accuracy, SatelliteCount};
use units::Radians;

/// A poq file reader.
#[derive(Debug)]
#[allow(missing_docs)]
pub struct Reader<R: Read + Seek> {
    pub avgint: f64,
    pub devint: f64,
    pub maxint: f64,
    pub version: Version,
    reader: R,
}

impl Reader<BufReader<File>> {
    /// Creates a new reader for the given path.
    ///
    /// # Examples
    ///
    /// ```
    /// use pos::poq::Reader;
    /// let reader = Reader::from_path("data/sbet_mission_1.poq").unwrap();
    /// ```
    pub fn from_path<P: AsRef<Path>>(path: P) -> Result<Reader<BufReader<File>>> {
        let reader = BufReader::new(try!(File::open(path)));
        Reader::new(reader)
    }
}

impl<R: Seek + Read> Reader<R> {
    fn new(mut reader: R) -> Result<Reader<R>> {
        let ref mut preamble = [0; 35];
        try!(read_full(&mut reader, preamble));

        let major = try!(reader.read_u16::<LittleEndian>());
        let minor = try!(reader.read_u16::<LittleEndian>());
        let version = Version::new(major, minor);
        let avgint = try!(reader.read_f64::<LittleEndian>());
        let maxint = try!(reader.read_f64::<LittleEndian>());
        let devint = try!(reader.read_f64::<LittleEndian>());

        Ok(Reader {
            avgint: avgint,
            devint: devint,
            maxint: maxint,
            reader: reader,
            version: version,
        })
    }

    /// Reads a record from this reader.
    ///
    /// # Examples
    ///
    /// ```
    /// use pos::poq::Reader;
    /// let mut reader = Reader::from_path("data/sbet_mission_1.poq").unwrap();
    /// let accuracy = reader.read_accuracy().unwrap();
    /// ```
    pub fn read_accuracy(&mut self) -> Result<Option<Accuracy>> {
        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)),
        };
        let north = try!(self.reader.read_f64::<LittleEndian>());
        let east = try!(self.reader.read_f64::<LittleEndian>());
        let down = try!(self.reader.read_f64::<LittleEndian>());
        let roll = try!(self.reader.read_f64::<LittleEndian>());
        let pitch = try!(self.reader.read_f64::<LittleEndian>());
        let yaw = try!(self.reader.read_f64::<LittleEndian>());
        let pdop = try!(self.reader.read_f64::<LittleEndian>());
        let satellite_count = if self.version.specifies_satellite_count() {
            let ngps = try!(self.reader.read_u16::<LittleEndian>());
            let nglonass = try!(self.reader.read_u16::<LittleEndian>());
            SatelliteCount::Specified {
                gps: ngps,
                glonass: nglonass,
            }
        } else {
            SatelliteCount::Unspecified(try!(self.reader.read_u16::<LittleEndian>()))
        };

        Ok(Some(Accuracy {
            time: time,
            y: north,
            x: east,
            z: down,
            roll: Radians::from_degrees(roll),
            pitch: Radians::from_degrees(pitch),
            yaw: Radians::from_degrees(yaw),
            pdop: pdop,
            satellite_count: Some(satellite_count),
        }))
    }
}

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

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

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

/// poq file version.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Version {
    major: u16,
    minor: u16,
}

impl Version {
    /// Creates a new version.
    ///
    /// # Examples
    ///
    /// ```
    /// use pos::poq::Version;
    /// Version::new(1, 1);
    /// ```
    pub fn new(major: u16, minor: u16) -> Version {
        Version {
            major: major,
            minor: minor,
        }
    }

    fn specifies_satellite_count(&self) -> bool {
        self.minor >= 1
    }
}

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

    #[test]
    fn iter() {
        let reader = Reader::from_path("data/sbet_mission_1.poq").unwrap();
        let records: Vec<_> = reader.into_iter().zip(0..5571).map(|(r, _)| r).collect();
        assert_eq!(5571, records.len());
    }
}