nema_parser/
gnss_multignss_parser.rs

1//! GNSS Multi-System NMEA Parser
2//!
3//! This module provides data structures and logic for parsing NMEA sentences from multiple GNSS systems
4//! (GPS, GLONASS, GALILEO, BEIDOU). It supports extracting satellite information, position, DOP values,
5//! and fusing positions from different systems for improved accuracy.
6//!
7//! # Features
8//! - Parses GGA, RMC, VTG, GSA, GSV, and GLL sentences for supported systems
9//! - Tracks satellite info and usage per system
10//! - Calculates fused position using weighted averaging and advanced filtering
11//! - Provides utility functions for latitude/longitude parsing
12//!
13//! # Usage
14//!
15//! ```rust
16//! use nema_parser::gnss_multignss_parser::GnssData;
17//! let mut gnss = GnssData::new();
18//! gnss.feed_nmea("$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
19//! gnss.calculate_fused_position();
20//! if let Some(fused) = &gnss.fused_position {
21//!     println!("Fused position: {}, {}", fused.latitude, fused.longitude);
22//! }
23//! ```
24
25use std::collections::HashMap;
26
27/// Information about a single satellite, including PRN, elevation, azimuth, and SNR.
28#[derive(Debug, Default, Clone)]
29pub struct SatelliteInfo {
30    /// Pseudo-Random Noise number (satellite identifier)
31    pub prn: u16,
32    /// Elevation angle in degrees
33    pub elevation: Option<u8>,
34    /// Azimuth angle in degrees
35    pub azimuth: Option<u16>,
36    /// Signal-to-noise ratio in dBHz
37    pub snr: Option<u8>,
38}
39
40/// Data for a single GNSS system (GPS, GLONASS, GALILEO, BEIDOU).
41#[derive(Debug, Default, Clone)]
42pub struct GnssSystemData {
43    /// List of satellites used for position fix
44    pub satellites_used: Vec<u16>,
45    /// Information about all tracked satellites
46    pub satellites_info: HashMap<u16, SatelliteInfo>,
47    /// Position Dilution of Precision
48    pub pdop: Option<f64>,
49    /// Horizontal Dilution of Precision
50    pub hdop: Option<f64>,
51    /// Vertical Dilution of Precision
52    pub vdop: Option<f64>,
53    /// Latitude in decimal degrees
54    pub latitude: Option<f64>,
55    /// Longitude in decimal degrees
56    pub longitude: Option<f64>,
57}
58
59/// Main GNSS data structure holding parsed information and fused position.
60#[derive(Debug, Default, Clone)]
61pub struct GnssData {
62    /// UTC time from NMEA sentence
63    pub time: Option<String>,
64    /// Latitude in decimal degrees
65    pub latitude: Option<f64>,
66    /// Longitude in decimal degrees
67    pub longitude: Option<f64>,
68    /// Fix quality indicator
69    pub fix_quality: Option<u8>,
70    /// Number of satellites used for fix
71    pub num_satellites: Option<u8>,
72    /// Altitude above mean sea level in meters
73    pub altitude: Option<f64>,
74    /// Speed over ground in knots
75    pub speed_knots: Option<f64>,
76    /// Track angle in degrees
77    pub track_angle: Option<f64>,
78    /// Date in DDMMYY format
79    pub date: Option<String>,
80    /// Data for each GNSS system
81    pub systems: HashMap<&'static str, GnssSystemData>,
82    /// Fused position calculated from available systems
83    pub fused_position: Option<FusedPosition>,
84}
85
86/// Fused position result from multiple GNSS systems.
87#[derive(Debug, Clone)]
88pub struct FusedPosition {
89    /// Fused latitude in decimal degrees
90    pub latitude: f64,
91    /// Fused longitude in decimal degrees
92    pub longitude: f64,
93    /// Estimated accuracy in meters
94    pub estimated_accuracy: f64,
95    /// List of contributing GNSS systems
96    pub contributing_systems: Vec<String>,
97}
98
99impl GnssData {
100    /// Creates a new `GnssData` instance with all supported GNSS systems initialized.
101    ///
102    /// # Example
103    /// ```
104    /// use nema_parser::gnss_multignss_parser::GnssData;
105    ///
106    /// let gnss = GnssData::new();
107    /// ```
108    pub fn new() -> Self {
109        let mut systems = HashMap::new();
110        systems.insert("GPS", GnssSystemData::default());
111        systems.insert("GLONASS", GnssSystemData::default());
112        systems.insert("GALILEO", GnssSystemData::default());
113        systems.insert("BEIDOU", GnssSystemData::default());
114        Self { systems, ..Default::default() }
115    }
116
117    /// Parses and updates GNSS data from a GGA sentence.
118    fn update_gga(&mut self, parts: &[&str]) {
119        let lat = parse_lat(parts.get(2), parts.get(3));
120        let lon = parse_lon(parts.get(4), parts.get(5));
121        self.time = parts.get(1).map(|s| s.to_string());
122        self.latitude = lat;
123        self.longitude = lon;
124        self.fix_quality = parts.get(6).and_then(|s| s.parse().ok());
125        self.num_satellites = parts.get(7).and_then(|s| s.parse().ok());
126        self.altitude = parts.get(9).and_then(|s| s.parse().ok());
127
128        // Update coordinates for all systems that have satellites
129        for (_, system_data) in self.systems.iter_mut() {
130            if system_data.satellites_info.len() > 0 {
131                system_data.latitude = lat;
132                system_data.longitude = lon;
133            } else {
134                system_data.latitude = None;
135                system_data.longitude = None;
136            }
137        }
138    }
139
140    /// Parses and updates GNSS data from an RMC sentence.
141    fn update_rmc(&mut self, parts: &[&str]) {
142        let lat = parse_lat(parts.get(3), parts.get(4));
143        let lon = parse_lon(parts.get(5), parts.get(6));
144        self.time = parts.get(1).map(|s| s.to_string());
145        self.latitude = lat;
146        self.longitude = lon;
147        self.speed_knots = parts.get(7).and_then(|s| s.parse().ok());
148        self.track_angle = parts.get(8).and_then(|s| s.parse().ok());
149        self.date = parts.get(9).map(|s| s.to_string());
150
151        // Update coordinates for all systems that have satellites
152        for (_, system_data) in self.systems.iter_mut() {
153            if system_data.satellites_info.len() > 0 {
154                system_data.latitude = lat;
155                system_data.longitude = lon;
156            } else {
157                system_data.latitude = None;
158                system_data.longitude = None;
159            }
160        }
161    }
162
163    /// Parses and updates GNSS data from a VTG sentence.
164    fn update_vtg(&mut self, parts: &[&str]) {
165        self.speed_knots = parts.get(5).and_then(|s| s.parse().ok());
166    }
167
168    /// Parses and updates GNSS system data from a GSA sentence.
169    fn update_gsa(&mut self, parts: &[&str]) {
170        let mut gps_ids = Vec::new();
171        for i in 3..=14 {
172            if let Some(Ok(prn)) = parts.get(i).map(|s| s.parse()) {
173                gps_ids.push(prn);
174            }
175        }
176
177        // Find DOP values by searching for the first three non-empty numeric fields after satellites
178        let mut dop_values = Vec::new();
179        for i in 15..parts.len() {
180            if let Some(part) = parts.get(i) {
181                let clean_part = part.split('*').next().unwrap_or(part);
182                if !clean_part.is_empty() {
183                    if let Ok(value) = clean_part.parse::<f64>() {
184                        dop_values.push(value);
185                        if dop_values.len() == 3 {
186                            break;
187                        }
188                    }
189                }
190            }
191        }
192
193        let pdop = dop_values.get(0).copied();
194        let hdop = dop_values.get(1).copied();
195        let vdop = dop_values.get(2).copied();
196
197        let mut updated_systems = Vec::new();
198        for prn in &gps_ids {
199            match prn {
200                1..=32 => {
201                    self.systems.get_mut("GPS").unwrap().satellites_used.push(*prn as u16);
202                    if !updated_systems.contains(&"GPS") {
203                        updated_systems.push("GPS");
204                    }
205                },
206                65..=96 => {
207                    self.systems.get_mut("GLONASS").unwrap().satellites_used.push(*prn as u16);
208                    if !updated_systems.contains(&"GLONASS") {
209                        updated_systems.push("GLONASS");
210                    }
211                },
212                201..=236 => {
213                    self.systems.get_mut("BEIDOU").unwrap().satellites_used.push(*prn as u16);
214                    if !updated_systems.contains(&"BEIDOU") {
215                        updated_systems.push("BEIDOU");
216                    }
217                },
218                301..=336 => {
219                    self.systems.get_mut("GALILEO").unwrap().satellites_used.push(*prn as u16);
220                    if !updated_systems.contains(&"GALILEO") {
221                        updated_systems.push("GALILEO");
222                    }
223                },
224                _ => {}
225            }
226        }
227        // Only update error values for systems that received satellites in this GSA sentence
228        for sys_name in updated_systems {
229            if let Some(sys) = self.systems.get_mut(sys_name) {
230                sys.pdop = pdop;
231                sys.hdop = hdop;
232                sys.vdop = vdop;
233            }
234        }
235    }
236
237    /// Parses and updates satellite information from a GSV sentence for the specified system.
238    fn update_gsv(&mut self, parts: &[&str], system: &str) {
239        if let Some(sys_data) = self.systems.get_mut(system) {
240            let mut i = 4;
241            while i + 3 < parts.len() {
242                if let Some(Ok(prn)) = parts.get(i).map(|s| s.parse()) {
243                    let elevation = parts.get(i + 1).and_then(|s| s.parse().ok());
244                    let azimuth = parts.get(i + 2).and_then(|s| s.parse().ok());
245                    let snr = parts.get(i + 3).and_then(|s| s.trim_end_matches('*').parse().ok());
246                    sys_data.satellites_info.insert(
247                        prn,
248                        SatelliteInfo {
249                            prn,
250                            elevation,
251                            azimuth,
252                            snr,
253                        },
254                    );
255                }
256                i += 4;
257            }
258        }
259    }
260
261    /// Parses and updates latitude/longitude from a GLL sentence for the specified system.
262    fn update_gll(&mut self, parts: &[&str], system: &str) {
263        let lat = parse_lat(parts.get(1), parts.get(2));
264        let lon = parse_lon(parts.get(3), parts.get(4));
265        self.latitude = lat;
266        self.longitude = lon;
267        if let Some(sys) = self.systems.get_mut(system) {
268            if sys.satellites_info.len() > 0 {
269                sys.latitude = lat;
270                sys.longitude = lon;
271            } else {
272                sys.latitude = None;
273                sys.longitude = None;
274            }
275        }
276    }
277
278    /// Feeds a single NMEA sentence to the parser and updates internal state.
279    ///
280    /// # Arguments
281    /// * `sentence` - A string slice containing the NMEA sentence.
282    ///
283    /// # Example
284    /// ```
285    /// use nema_parser::gnss_multignss_parser::GnssData;
286    /// let mut gnss = GnssData::new();
287    /// gnss.feed_nmea("$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
288    /// ```
289    pub fn feed_nmea(&mut self, sentence: &str) {
290        let sentence = sentence.trim_start_matches('$');
291        let parts: Vec<&str> = sentence.split(',').collect();
292        match parts.first().filter(|s| s.len() >= 5).map(|s| &s[0..5]) {
293            Some("GNGGA") => self.update_gga(&parts),
294            Some("GNRMC") => self.update_rmc(&parts),
295            Some("GNVTG") => self.update_vtg(&parts),
296            Some("GNGSA") => self.update_gsa(&parts),
297            Some("GPGSV") => self.update_gsv(&parts, "GPS"),
298            Some("GLGSV") => self.update_gsv(&parts, "GLONASS"),
299            Some("GAGSV") => self.update_gsv(&parts, "GALILEO"),
300            Some("BDGSV") => self.update_gsv(&parts, "BEIDOU"),
301            Some("GPGLL") => self.update_gll(&parts, "GPS"),
302            Some("GLGLL") => self.update_gll(&parts, "GLONASS"),
303            Some("GAGLL") => self.update_gll(&parts, "GALILEO"),
304            Some("BDGLL") => self.update_gll(&parts, "BEIDOU"),
305            _ => {}
306        }
307    }
308
309    /// Calculates a fused position from all available GNSS systems using weighted averaging.
310    ///
311    /// The fused position is stored in `self.fused_position`.
312    ///
313    /// # Example
314    /// ```
315    /// use nema_parser::gnss_multignss_parser::GnssData;
316    /// let mut gnss = GnssData::new();
317    /// gnss.calculate_fused_position();
318    /// if let Some(fused) = &gnss.fused_position {
319    ///     println!("Fused position: {}, {}", fused.latitude, fused.longitude);
320    /// }
321    /// ```
322    pub fn calculate_fused_position(&mut self) {
323        let mut valid_positions = Vec::new();
324
325        for (system_name, system_data) in &self.systems {
326            if let (Some(lat), Some(lon), Some(hdop)) = (system_data.latitude, system_data.longitude, system_data.hdop) {
327                if system_data.satellites_info.len() >= 4 { // Minimum satellites for 3D fix
328                    valid_positions.push((system_name.to_string(), lat, lon, hdop));
329                }
330            }
331        }
332
333        if valid_positions.is_empty() {
334            self.fused_position = None;
335            return;
336        }
337
338        if valid_positions.len() == 1 {
339            let (system, lat, lon, hdop) = &valid_positions[0];
340            self.fused_position = Some(FusedPosition {
341                latitude: *lat,
342                longitude: *lon,
343                estimated_accuracy: hdop * 3.0, // Rough accuracy estimate in meters
344                contributing_systems: vec![system.clone()],
345            });
346            return;
347        }
348
349        // Weighted average using inverse HDOP as weights
350        let mut weighted_lat = 0.0;
351        let mut weighted_lon = 0.0;
352        let mut total_weight = 0.0;
353        let mut contributing_systems = Vec::new();
354
355        for (system, lat, lon, hdop) in &valid_positions {
356            let weight = 1.0 / (hdop + 0.1); // Add small value to avoid division by zero
357            weighted_lat += lat * weight;
358            weighted_lon += lon * weight;
359            total_weight += weight;
360            contributing_systems.push(system.clone());
361        }
362
363        if total_weight > 0.0 {
364            let fused_lat = weighted_lat / total_weight;
365            let fused_lon = weighted_lon / total_weight;
366
367            // Calculate estimated accuracy based on weighted HDOP
368            let weighted_hdop: f64 = valid_positions.iter()
369                .map(|(_, _, _, hdop)| {
370                    let weight = 1.0 / (hdop + 0.1);
371                    hdop * weight
372                })
373                .sum::<f64>() / total_weight;
374
375            self.fused_position = Some(FusedPosition {
376                latitude: fused_lat,
377                longitude: fused_lon,
378                estimated_accuracy: weighted_hdop * 2.0, // Improved accuracy estimate
379                contributing_systems,
380            });
381        } else {
382            self.fused_position = None;
383        }
384    }
385
386    /// Calculates an advanced fused position using a Kalman-like filtering approach.
387    ///
388    /// The fused position is stored in `self.fused_position`.
389    pub fn calculate_advanced_fused_position(&mut self) {
390        let mut valid_positions = Vec::new();
391
392        for (system_name, system_data) in &self.systems {
393            if let (Some(lat), Some(lon), Some(hdop), Some(pdop)) =
394                (system_data.latitude, system_data.longitude, system_data.hdop, system_data.pdop) {
395                if system_data.satellites_info.len() >= 4 {
396                    valid_positions.push((system_name.to_string(), lat, lon, hdop, pdop));
397                }
398            }
399        }
400
401        if valid_positions.is_empty() {
402            self.fused_position = None;
403            return;
404        }
405
406        // Kalman-like filtering approach
407        let mut weighted_lat = 0.0;
408        let mut weighted_lon = 0.0;
409        let mut total_weight = 0.0;
410        let mut contributing_systems = Vec::new();
411
412        for (system, lat, lon, hdop, pdop) in &valid_positions {
413            // Combined weight using both HDOP and PDOP
414            let combined_dop = (hdop * hdop + pdop * pdop).sqrt();
415            let weight = 1.0 / (combined_dop + 0.1);
416
417            weighted_lat += lat * weight;
418            weighted_lon += lon * weight;
419            total_weight += weight;
420            contributing_systems.push(system.clone());
421        }
422
423        if total_weight > 0.0 {
424            let fused_lat = weighted_lat / total_weight;
425            let fused_lon = weighted_lon / total_weight;
426
427            // Calculate confidence interval
428            let variance: f64 = valid_positions.iter()
429                .map(|(_, lat, lon, hdop, _)| {
430                    let weight = 1.0 / (*hdop + 0.1);
431                    let lat_diff = lat - fused_lat;
432                    let lon_diff = lon - fused_lon;
433                    weight * (lat_diff * lat_diff + lon_diff * lon_diff)
434                })
435                .sum::<f64>() / total_weight;
436
437            let estimated_accuracy = variance.sqrt() * 111000.0; // Convert to meters roughly
438
439            self.fused_position = Some(FusedPosition {
440                latitude: fused_lat,
441                longitude: fused_lon,
442                estimated_accuracy: estimated_accuracy.max(1.0), // Minimum 1 meter accuracy
443                contributing_systems,
444            });
445        } else {
446            self.fused_position = None;
447        }
448    }
449}
450
451/// Parses latitude from NMEA format to decimal degrees.
452///
453/// # Arguments
454/// * `value` - Latitude value as string (DDMM.MMMM)
455/// * `hemi` - Hemisphere ("N" or "S")
456///
457/// # Returns
458/// * `Option<f64>` - Latitude in decimal degrees
459fn parse_lat(value: Option<&&str>, hemi: Option<&&str>) -> Option<f64> {
460    let val = value?.parse::<f64>().ok()?;
461    let deg = (val / 100.0).floor();
462    let min = val % 100.0;
463    let mut result = deg + min / 60.0;
464    if hemi? == &"S" { result *= -1.0; }
465    Some(result)
466}
467
468/// Parses longitude from NMEA format to decimal degrees.
469///
470/// # Arguments
471/// * `value` - Longitude value as string (DDDMM.MMMM)
472/// * `hemi` - Hemisphere ("E" or "W")
473///
474/// # Returns
475/// * `Option<f64>` - Longitude in decimal degrees
476fn parse_lon(value: Option<&&str>, hemi: Option<&&str>) -> Option<f64> {
477    let val = value?.parse::<f64>().ok()?;
478    let deg = (val / 100.0).floor();
479    let min = val % 100.0;
480    let mut result = deg + min / 60.0;
481    if hemi? == &"W" { result *= -1.0; }
482    Some(result)
483}
484
485#[cfg(test)]
486mod tests {
487    use super::*;
488
489    #[test]
490    fn test_gnssdata_new() {
491        let gnss = GnssData::new();
492        assert!(gnss.systems.contains_key("GPS"));
493        assert!(gnss.systems.contains_key("GLONASS"));
494        assert!(gnss.systems.contains_key("GALILEO"));
495        assert!(gnss.systems.contains_key("BEIDOU"));
496    }
497
498    #[test]
499    fn test_feed_nmea_gga() {
500        let mut gnss = GnssData::new();
501        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
502        gnss.feed_nmea(gga);
503        assert_eq!(gnss.time, Some("123519".to_string()));
504        assert!(gnss.latitude.is_some());
505        assert!(gnss.longitude.is_some());
506        assert_eq!(gnss.fix_quality, Some(1));
507        assert_eq!(gnss.num_satellites, Some(8));
508        assert_eq!(gnss.altitude, Some(545.4));
509    }
510
511    #[test]
512    fn test_feed_nmea_gps_gsv() {
513        let mut gnss = GnssData::new();
514        let gsv = "$GPGSV,2,1,08,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*7C";
515        gnss.feed_nmea(gsv);
516        let gps_info = &gnss.systems["GPS"].satellites_info;
517        assert_eq!(gps_info.len(), 4);
518        assert!(gps_info.contains_key(&1));
519        assert!(gps_info.contains_key(&2));
520        assert!(gps_info.contains_key(&3));
521        assert!(gps_info.contains_key(&4));
522        let sat1 = gps_info.get(&1).unwrap();
523        assert_eq!(sat1.prn, 1);
524        assert_eq!(sat1.elevation, Some(40));
525        assert_eq!(sat1.azimuth, Some(83));
526        assert_eq!(sat1.snr, Some(41));
527    }
528
529    #[test]
530    fn test_feed_nmea_glonass_gsv() {
531        let mut gnss = GnssData::new();
532        let gsv = "$GLGSV,2,1,08,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*61";
533        gnss.feed_nmea(gsv);
534        let glonass_info = &gnss.systems["GLONASS"].satellites_info;
535        assert_eq!(glonass_info.len(), 4);
536        assert!(glonass_info.contains_key(&67));
537        assert!(glonass_info.contains_key(&68));
538        assert!(glonass_info.contains_key(&69));
539        assert!(glonass_info.contains_key(&77));
540        let sat67 = glonass_info.get(&67).unwrap();
541        assert_eq!(sat67.prn, 67);
542        assert_eq!(sat67.elevation, Some(14));
543        assert_eq!(sat67.azimuth, Some(186));
544        assert_eq!(sat67.snr, Some(9));
545    }
546
547    #[test]
548    fn test_feed_nmea_galileo_gsv() {
549        let mut gnss = GnssData::new();
550        let gsv = "$GAGSV,1,1,04,301,45,123,35,302,30,045,40,303,60,234,45,304,25,156,38*XX";
551        gnss.feed_nmea(gsv);
552        let galileo_info = &gnss.systems["GALILEO"].satellites_info;
553        assert_eq!(galileo_info.len(), 4);
554        assert!(galileo_info.contains_key(&301));
555        assert!(galileo_info.contains_key(&302));
556        assert!(galileo_info.contains_key(&303));
557        assert!(galileo_info.contains_key(&304));
558        let sat301 = galileo_info.get(&301).unwrap();
559        assert_eq!(sat301.prn, 301);
560        assert_eq!(sat301.elevation, Some(45));
561        assert_eq!(sat301.azimuth, Some(123));
562        assert_eq!(sat301.snr, Some(35));
563    }
564
565    #[test]
566    fn test_feed_nmea_beidou_gsv() {
567        let mut gnss = GnssData::new();
568        let gsv = "$BDGSV,1,1,04,201,45,123,35,202,30,045,40,203,60,234,45,204,25,156,38*XX";
569        gnss.feed_nmea(gsv);
570        let beidou_info = &gnss.systems["BEIDOU"].satellites_info;
571        assert_eq!(beidou_info.len(), 4);
572        assert!(beidou_info.contains_key(&201));
573        assert!(beidou_info.contains_key(&202));
574        assert!(beidou_info.contains_key(&203));
575        assert!(beidou_info.contains_key(&204));
576        let sat201 = beidou_info.get(&201).unwrap();
577        assert_eq!(sat201.prn, 201);
578        assert_eq!(sat201.elevation, Some(45));
579        assert_eq!(sat201.azimuth, Some(123));
580        assert_eq!(sat201.snr, Some(35));
581    }
582
583    #[test]
584    fn test_feed_nmea_gsa_gps() {
585        let mut gnss = GnssData::new();
586        let gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,09,10,11,12,1.2,0.9,2.1*39";
587        gnss.feed_nmea(gsa);
588        let gps_used = &gnss.systems["GPS"].satellites_used;
589        assert!(gps_used.contains(&1));
590        assert!(gps_used.contains(&2));
591        assert!(gps_used.contains(&3));
592        assert!(gps_used.contains(&4));
593        assert_eq!(gnss.systems["GPS"].pdop, Some(1.2));
594        assert_eq!(gnss.systems["GPS"].hdop, Some(0.9));
595        assert_eq!(gnss.systems["GPS"].vdop, Some(2.1));
596    }
597
598    #[test]
599    fn test_feed_nmea_gsa_glonass() {
600        let mut gnss = GnssData::new();
601        let gsa = "$GNGSA,A,3,67,68,69,77,78,79,86,87,88,,,,,1.8,1.1,1.4*3F";
602        gnss.feed_nmea(gsa);
603
604        // Debug output
605        println!("GLONASS satellites_used: {:?}", gnss.systems["GLONASS"].satellites_used);
606        println!("GLONASS pdop: {:?}", gnss.systems["GLONASS"].pdop);
607        println!("GLONASS hdop: {:?}", gnss.systems["GLONASS"].hdop);
608        println!("GLONASS vdop: {:?}", gnss.systems["GLONASS"].vdop);
609
610        let glonass_used = &gnss.systems["GLONASS"].satellites_used;
611        assert!(glonass_used.contains(&67));
612        assert!(glonass_used.contains(&68));
613        assert!(glonass_used.contains(&69));
614        assert!(glonass_used.contains(&77));
615        assert_eq!(gnss.systems["GLONASS"].pdop, Some(1.8));
616        assert_eq!(gnss.systems["GLONASS"].hdop, Some(1.1));
617        assert_eq!(gnss.systems["GLONASS"].vdop, Some(1.4));
618    }
619
620    #[test]
621    fn test_feed_nmea_gsa_galileo() {
622        let mut gnss = GnssData::new();
623        let gsa = "$GNGSA,A,3,301,302,303,304,305,306,,,,,,,2.1,1.3,1.6*XX";
624        gnss.feed_nmea(gsa);
625        let galileo_used = &gnss.systems["GALILEO"].satellites_used;
626        assert!(galileo_used.contains(&301));
627        assert!(galileo_used.contains(&302));
628        assert!(galileo_used.contains(&303));
629        assert!(galileo_used.contains(&304));
630        assert_eq!(gnss.systems["GALILEO"].pdop, Some(2.1));
631        assert_eq!(gnss.systems["GALILEO"].hdop, Some(1.3));
632        assert_eq!(gnss.systems["GALILEO"].vdop, Some(1.6));
633    }
634
635    #[test]
636    fn test_feed_nmea_gsa_beidou() {
637        let mut gnss = GnssData::new();
638        let gsa = "$GNGSA,A,3,201,202,203,204,205,206,,,,,,,1.5,0.8,1.2*XX";
639        gnss.feed_nmea(gsa);
640        let beidou_used = &gnss.systems["BEIDOU"].satellites_used;
641        assert!(beidou_used.contains(&201));
642        assert!(beidou_used.contains(&202));
643        assert!(beidou_used.contains(&203));
644        assert!(beidou_used.contains(&204));
645        assert_eq!(gnss.systems["BEIDOU"].pdop, Some(1.5));
646        assert_eq!(gnss.systems["BEIDOU"].hdop, Some(0.8));
647        assert_eq!(gnss.systems["BEIDOU"].vdop, Some(1.2));
648    }
649
650    #[test]
651    fn test_coordinates_update_for_systems_with_satellites() {
652        let mut gnss = GnssData::new();
653
654        // Add GPS satellites
655        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
656        gnss.feed_nmea(gps_gsv);
657
658        // Add GLONASS satellites
659        let glonass_gsv = "$GLGSV,1,1,04,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*XX";
660        gnss.feed_nmea(glonass_gsv);
661
662        // Update coordinates via GGA
663        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
664        gnss.feed_nmea(gga);
665
666        // Systems with satellites should have coordinates
667        assert!(gnss.systems["GPS"].latitude.is_some());
668        assert!(gnss.systems["GPS"].longitude.is_some());
669        assert!(gnss.systems["GLONASS"].latitude.is_some());
670        assert!(gnss.systems["GLONASS"].longitude.is_some());
671
672        // Systems without satellites should not have coordinates
673        assert!(gnss.systems["GALILEO"].latitude.is_none());
674        assert!(gnss.systems["GALILEO"].longitude.is_none());
675        assert!(gnss.systems["BEIDOU"].latitude.is_none());
676        assert!(gnss.systems["BEIDOU"].longitude.is_none());
677    }
678
679    #[test]
680    fn test_fused_position_calculation() {
681        let mut gnss = GnssData::new();
682
683        // Add GPS satellites and coordinates
684        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
685        gnss.feed_nmea(gps_gsv);
686        let gps_gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,,,,,1.2,0.9,2.1*39";
687        gnss.feed_nmea(gps_gsa);
688
689        // Add GLONASS satellites and coordinates
690        let glonass_gsv = "$GLGSV,1,1,04,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*XX";
691        gnss.feed_nmea(glonass_gsv);
692        let glonass_gsa = "$GNGSA,A,3,67,68,69,77,78,79,86,87,,,,,1.8,1.1,1.4*3F";
693        gnss.feed_nmea(glonass_gsa);
694
695        // Update coordinates
696        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
697        gnss.feed_nmea(gga);
698
699        // Calculate fused position
700        gnss.calculate_fused_position();
701
702        assert!(gnss.fused_position.is_some());
703        let fused = gnss.fused_position.as_ref().unwrap();
704        assert!(fused.contributing_systems.contains(&"GPS".to_string()));
705        assert!(fused.contributing_systems.contains(&"GLONASS".to_string()));
706        assert!(fused.estimated_accuracy > 0.0);
707    }
708
709    #[test]
710    fn test_parse_lat_lon() {
711        let lat = parse_lat(Some(&"4807.038"), Some(&"N"));
712        let lon = parse_lon(Some(&"01131.000"), Some(&"E"));
713        assert!(lat.is_some());
714        assert!(lon.is_some());
715        let lat_val = lat.unwrap();
716        let lon_val = lon.unwrap();
717        assert!((lat_val - 48.1173).abs() < 0.0001);
718        assert!((lon_val - 11.5166667).abs() < 0.0001);
719    }
720}
nema_parser/gnss_multignss_parser.rs

nema_parser/
gnss_multignss_parser.rs
