ados-positioning 0.1.0

Split a mixed UBX, NMEA and RTCM3 byte stream with resync, build u-blox VALSET commands and run an RTK base survey-in state machine.
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

//! NMEA GGA / RMC decode, the fallback when a receiver sends no UBX.

use crate::receiver::Fix;

/// A decoded GGA sentence.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Gga {
    /// Fix quality from the GGA quality field.
    pub fix: Fix,
    /// Satellites used.
    pub sats: u8,
    /// Horizontal dilution of precision.
    pub hdop: Option<f64>,
    /// Latitude, degrees.
    pub lat_deg: Option<f64>,
    /// Longitude, degrees.
    pub lon_deg: Option<f64>,
    /// Altitude above mean sea level, metres.
    pub h_msl_m: Option<f64>,
    /// Height above the ellipsoid (MSL plus geoid separation), metres.
    pub h_ellipsoid_m: Option<f64>,
}

/// A decoded RMC sentence.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Rmc {
    /// Fix from the RMC status and mode fields.
    pub fix: Fix,
    /// Latitude, degrees.
    pub lat_deg: Option<f64>,
    /// Longitude, degrees.
    pub lon_deg: Option<f64>,
}

/// A sentence type this crate decodes.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum Sentence {
    /// GGA: fix data.
    Gga(Gga),
    /// RMC: recommended minimum data.
    Rmc(Rmc),
}

/// Parse a checksum-verified sentence (`$GNGGA,...*hh`).
pub fn parse(line: &str) -> Option<Sentence> {
    let body = line.strip_prefix('$')?;
    let body = body.rsplit_once('*').map_or(body, |(b, _)| b);
    let f: Vec<&str> = body.split(',').collect();
    let kind = f.first()?.get(2..)?;
    match kind {
        "GGA" if f.len() >= 12 => Some(Sentence::Gga(gga(&f))),
        "RMC" if f.len() >= 7 => Some(Sentence::Rmc(rmc(&f))),
        _ => None,
    }
}

fn num(s: &str) -> Option<f64> {
    s.parse().ok().filter(|v: &f64| v.is_finite())
}

/// `ddmm.mmmm` / `dddmm.mmmm` with a hemisphere letter.
fn coord(v: &str, hemi: &str) -> Option<f64> {
    let raw = num(v)?;
    let deg = (raw / 100.0).trunc();
    let val = deg + (raw - deg * 100.0) / 60.0;
    match hemi {
        "N" | "E" => Some(val),
        "S" | "W" => Some(-val),
        _ => None,
    }
}

fn gga(f: &[&str]) -> Gga {
    let fix = match f[6] {
        "1" => Fix::ThreeD,
        "2" => Fix::Dgps,
        "4" => Fix::RtkFixed,
        "5" => Fix::RtkFloat,
        _ => Fix::None,
    };
    let h_msl = num(f[9]);
    let sep = num(f[11]);
    let has_fix = fix != Fix::None;
    Gga {
        fix,
        sats: f[7].parse().unwrap_or(0),
        hdop: num(f[8]),
        lat_deg: coord(f[2], f[3]).filter(|_| has_fix),
        lon_deg: coord(f[4], f[5]).filter(|_| has_fix),
        h_msl_m: h_msl.filter(|_| has_fix),
        h_ellipsoid_m: h_msl.zip(sep).map(|(h, s)| h + s).filter(|_| has_fix),
    }
}

fn rmc(f: &[&str]) -> Rmc {
    // RMC cannot tell 2D from 3D; a valid autonomous fix reads as 2d.
    let mode = f.get(12).copied().unwrap_or("A");
    let fix = match (f[2], mode) {
        ("A", "D") => Fix::Dgps,
        ("A", "R") => Fix::RtkFixed,
        ("A", "F") => Fix::RtkFloat,
        ("A", "N") => Fix::None,
        ("A", _) => Fix::TwoD,
        _ => Fix::None,
    };
    let has_fix = fix != Fix::None;
    Rmc {
        fix,
        lat_deg: coord(f[3], f[4]).filter(|_| has_fix),
        lon_deg: coord(f[5], f[6]).filter(|_| has_fix),
    }
}