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//! # Altitude Reference
8//!
9//! All altitude values (e.g., `altitude` fields in structs and fused position) are **above mean sea level** (MSL),
10//! as reported by NMEA GGA sentences.
11//!
12//! # Features
13//! - Parses GGA, RMC, VTG, GSA, GSV, and GLL sentences for supported systems
14//! - Tracks satellite info and usage per system
15//! - Calculates fused position using weighted averaging and advanced filtering
16//! - Provides utility functions for latitude/longitude parsing
17//!
18//! # Usage
19//!
20//! ```rust
21//! use nema_parser::gnss_multignss_parser::GnssData;
22//! let mut gnss = GnssData::new();
23//! gnss.feed_nmea("$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
24//! gnss.calculate_fused_position();
25//! if let Some(fused) = &gnss.fused_position {
26//!     println!("Fused position: {}, {}", fused.latitude, fused.longitude);
27//!     println!("Altitude above mean sea level: {}", fused.altitude);
28//! }
29//! ```
30
31use std::collections::HashMap;
32
33/// Information about a single satellite, including PRN, elevation, azimuth, and SNR.
34#[derive(Debug, Default, Clone)]
35pub struct SatelliteInfo {
36    /// Pseudo-Random Noise number (satellite identifier)
37    pub prn: u16,
38    /// Elevation angle in degrees
39    pub elevation: Option<u8>,
40    /// Azimuth angle in degrees
41    pub azimuth: Option<u16>,
42    /// Signal-to-noise ratio in dBHz
43    pub snr: Option<u8>,
44}
45
46/// Data for a single GNSS system (GPS, GLONASS, GALILEO, BEIDOU).
47#[derive(Debug, Default, Clone)]
48pub struct GnssSystemData {
49    /// List of satellites used for position fix
50    pub satellites_used: Vec<u16>,
51    /// Information about all tracked satellites
52    pub satellites_info: HashMap<u16, SatelliteInfo>,
53    /// Position Dilution of Precision
54    pub pdop: Option<f64>,
55    /// Horizontal Dilution of Precision
56    pub hdop: Option<f64>,
57    /// Vertical Dilution of Precision
58    pub vdop: Option<f64>,
59    /// Latitude in decimal degrees
60    pub latitude: Option<f64>,
61    /// Longitude in decimal degrees
62    pub longitude: Option<f64>,
63    /// Altitude above mean sea level in meters
64    pub altitude: Option<f64>,
65    /// Fixed accuracy in meters (best-case for this system)
66    pub fixed_accuracy: f64,
67    /// Module accuracy in meters (dynamically updated)
68    pub accuracy: f64,
69}
70
71
72/// Main GNSS data structure holding parsed information and fused position.
73#[derive(Debug, Default, Clone)]
74pub struct GnssData {
75    /// UTC time from NMEA sentence
76    pub time: Option<String>,
77    /// Latitude in decimal degrees
78    pub latitude: Option<f64>,
79    /// Longitude in decimal degrees
80    pub longitude: Option<f64>,
81    /// Fix quality indicator
82    pub fix_quality: Option<u8>,
83    /// Number of satellites used for fix
84    pub num_satellites: Option<u8>,
85    /// Altitude above mean sea level in meters
86    pub altitude: Option<f64>,
87    /// Speed over ground in knots
88    pub speed_knots: Option<f64>,
89    /// Track angle in degrees
90    pub track_angle: Option<f64>,
91    /// Date in DDMMYY format
92    pub date: Option<String>,
93    /// Data for each GNSS system
94    pub systems: HashMap<&'static str, GnssSystemData>,
95    /// Fused position calculated from available systems
96    pub fused_position: Option<FusedPosition>,
97}
98
99/// Fused position result from multiple GNSS systems.
100#[derive(Debug, Clone)]
101pub struct FusedPosition {
102    /// Fused latitude in decimal degrees
103    pub latitude: f64,
104    /// Fused longitude in decimal degrees
105    pub longitude: f64,
106    /// Fused altitude above mean sea level in meters
107    pub altitude: f64,
108    /// Estimated horizontal accuracy in meters
109    pub estimated_accuracy: f64,
110    /// Estimated altitude accuracy in meters
111    pub altitude_accuracy: f64,
112    /// List of contributing GNSS systems
113    pub contributing_systems: Vec<String>,
114}
115
116impl GnssData {
117    /// Creates a new `GnssData` instance with all supported GNSS systems initialized.
118    ///
119    /// # Example
120    /// ```
121    /// use nema_parser::gnss_multignss_parser::GnssData;
122    ///
123    /// let gnss = GnssData::new();
124    /// ```
125    pub fn new() -> Self {
126        let mut systems = HashMap::new();
127
128        // Initialize GPS with 2.0m fixed accuracy
129        let gps_system = GnssSystemData { fixed_accuracy: 2.0, accuracy: 2.0, ..Default::default() };
130        systems.insert("GPS", gps_system);
131
132        // Initialize GLONASS with 4.0m fixed accuracy
133        let glonass_system = GnssSystemData { fixed_accuracy: 4.0, accuracy: 4.0, ..Default::default() };
134        systems.insert("GLONASS", glonass_system);
135
136        // Initialize GALILEO with 3.0m fixed accuracy
137        let galileo_system = GnssSystemData { fixed_accuracy: 3.0, accuracy: 3.0, ..Default::default() };
138        systems.insert("GALILEO", galileo_system);
139
140        // Initialize BEIDOU with 3.0m fixed accuracy
141        let beidou_system = GnssSystemData { fixed_accuracy: 3.0, accuracy: 3.0, ..Default::default() };
142        systems.insert("BEIDOU", beidou_system);
143
144        Self {
145            systems,
146            ..Default::default()
147        }
148    }
149
150    /// Parses and updates GNSS data from a GGA sentence.
151    fn update_gga(&mut self, parts: &[&str]) {
152        let lat = parse_lat(parts.get(2), parts.get(3));
153        let lon = parse_lon(parts.get(4), parts.get(5));
154        let altitude = parts.get(9).and_then(|s| s.parse().ok());
155
156        self.time = parts.get(1).map(|s| s.to_string());
157        self.latitude = lat;
158        self.longitude = lon;
159        self.fix_quality = parts.get(6).and_then(|s| s.parse().ok());
160        self.num_satellites = parts.get(7).and_then(|s| s.parse().ok());
161        self.altitude = altitude;
162
163        // Update coordinates and altitude for all systems that have satellites
164        for (_, system_data) in self.systems.iter_mut() {
165            if !system_data.satellites_info.is_empty() {
166                system_data.latitude = lat;
167                system_data.longitude = lon;
168                system_data.altitude = altitude;
169            } else {
170                system_data.latitude = None;
171                system_data.longitude = None;
172                system_data.altitude = None;
173            }
174        }
175    }
176
177    /// Parses and updates GNSS data from an RMC sentence.
178    fn update_rmc(&mut self, parts: &[&str]) {
179        let lat = parse_lat(parts.get(3), parts.get(4));
180        let lon = parse_lon(parts.get(5), parts.get(6));
181        self.time = parts.get(1).map(|s| s.to_string());
182        self.latitude = lat;
183        self.longitude = lon;
184        self.speed_knots = parts.get(7).and_then(|s| s.parse().ok());
185        self.track_angle = parts.get(8).and_then(|s| s.parse().ok());
186        self.date = parts.get(9).map(|s| s.to_string());
187
188        // Update coordinates for all systems that have satellites
189        for (_, system_data) in self.systems.iter_mut() {
190            if !system_data.satellites_info.is_empty() {
191                system_data.latitude = lat;
192                system_data.longitude = lon;
193            } else {
194                system_data.latitude = None;
195                system_data.longitude = None;
196            }
197        }
198    }
199
200    /// Parses and updates GNSS data from a VTG sentence.
201    fn update_vtg(&mut self, parts: &[&str]) {
202        self.speed_knots = parts.get(5).and_then(|s| s.parse().ok());
203    }
204
205    /// Parses and updates GNSS system data from a GSA sentence.
206    fn update_gsa(&mut self, parts: &[&str]) {
207        let mut gps_ids = Vec::new();
208        for i in 3..=14 {
209            if let Some(Ok(prn)) = parts.get(i).map(|s| s.parse()) {
210                gps_ids.push(prn);
211            }
212        }
213
214        // Find DOP values by searching for the first three non-empty numeric fields after satellites
215        let mut dop_values = Vec::new();
216        for i in 15..parts.len() {
217            if let Some(part) = parts.get(i) {
218                let clean_part = part.split('*').next().unwrap_or(part);
219                if !clean_part.is_empty() {
220                    if let Ok(value) = clean_part.parse::<f64>() {
221                        dop_values.push(value);
222                        if dop_values.len() == 3 {
223                            break;
224                        }
225                    }
226                }
227            }
228        }
229
230        let pdop = dop_values.first().copied();
231        let hdop = dop_values.get(1).copied();
232        let vdop = dop_values.get(2).copied();
233
234        let mut updated_systems = Vec::new();
235        for prn in &gps_ids {
236            match prn {
237                1..=32 => {
238                    self.systems.get_mut("GPS").unwrap().satellites_used.push(*prn as u16);
239                    if !updated_systems.contains(&"GPS") {
240                        updated_systems.push("GPS");
241                    }
242                },
243                65..=96 => {
244                    self.systems.get_mut("GLONASS").unwrap().satellites_used.push(*prn as u16);
245                    if !updated_systems.contains(&"GLONASS") {
246                        updated_systems.push("GLONASS");
247                    }
248                },
249                201..=236 => {
250                    self.systems.get_mut("BEIDOU").unwrap().satellites_used.push(*prn as u16);
251                    if !updated_systems.contains(&"BEIDOU") {
252                        updated_systems.push("BEIDOU");
253                    }
254                },
255                301..=336 => {
256                    self.systems.get_mut("GALILEO").unwrap().satellites_used.push(*prn as u16);
257                    if !updated_systems.contains(&"GALILEO") {
258                        updated_systems.push("GALILEO");
259                    }
260                },
261                _ => {}
262            }
263        }
264        // Only update error values for systems that received satellites in this GSA sentence
265        for sys_name in updated_systems {
266            if let Some(sys) = self.systems.get_mut(sys_name) {
267                sys.pdop = pdop;
268                sys.hdop = hdop;
269                sys.vdop = vdop;
270                // Dynamically update accuracy using HDOP and fixed_accuracy
271                if let Some(hdop_val) = hdop {
272                    sys.accuracy = hdop_val * sys.fixed_accuracy;
273                } else {
274                    sys.accuracy = sys.fixed_accuracy;
275                }
276            }
277        }
278    }
279
280    /// Parses and updates satellite information from a GSV sentence for the specified system.
281    fn update_gsv(&mut self, parts: &[&str], system: &str) {
282        if let Some(sys_data) = self.systems.get_mut(system) {
283            let mut i = 4;
284            while i + 3 < parts.len() {
285                if let Some(Ok(prn)) = parts.get(i).map(|s| s.parse()) {
286                    let elevation = parts.get(i + 1).and_then(|s| s.parse().ok());
287                    let azimuth = parts.get(i + 2).and_then(|s| s.parse().ok());
288                    let snr = parts.get(i + 3).and_then(|s| s.trim_end_matches('*').parse().ok());
289                    sys_data.satellites_info.insert(
290                        prn,
291                        SatelliteInfo {
292                            prn,
293                            elevation,
294                            azimuth,
295                            snr,
296                        },
297                    );
298                }
299                i += 4;
300            }
301        }
302    }
303
304    /// Parses and updates latitude/longitude from a GLL sentence for the specified system.
305    fn update_gll(&mut self, parts: &[&str], system: &str) {
306        let lat = parse_lat(parts.get(1), parts.get(2));
307        let lon = parse_lon(parts.get(3), parts.get(4));
308        self.latitude = lat;
309        self.longitude = lon;
310        if let Some(sys) = self.systems.get_mut(system) {
311            if !sys.satellites_info.is_empty() {
312                sys.latitude = lat;
313                sys.longitude = lon;
314            } else {
315                sys.latitude = None;
316                sys.longitude = None;
317            }
318        }
319    }
320
321    /// Feeds a single NMEA sentence to the parser and updates internal state.
322    ///
323    /// # Arguments
324    /// * `sentence` - A string slice containing the NMEA sentence.
325    ///
326    /// # Example
327    /// ```
328    /// use nema_parser::gnss_multignss_parser::GnssData;
329    /// let mut gnss = GnssData::new();
330    /// gnss.feed_nmea("$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
331    /// ```
332    pub fn feed_nmea(&mut self, sentence: &str) {
333        let sentence = sentence.trim_start_matches('$');
334        let parts: Vec<&str> = sentence.split(',').collect();
335        match parts.first().filter(|s| s.len() >= 5).map(|s| &s[0..5]) {
336            Some("GNGGA") => self.update_gga(&parts),
337            Some("GNRMC") => self.update_rmc(&parts),
338            Some("GNVTG") => self.update_vtg(&parts),
339            Some("GNGSA") => self.update_gsa(&parts),
340            Some("GPGSV") => self.update_gsv(&parts, "GPS"),
341            Some("GLGSV") => self.update_gsv(&parts, "GLONASS"),
342            Some("GAGSV") => self.update_gsv(&parts, "GALILEO"),
343            Some("BDGSV") => self.update_gsv(&parts, "BEIDOU"),
344            Some("GPGLL") => self.update_gll(&parts, "GPS"),
345            Some("GLGLL") => self.update_gll(&parts, "GLONASS"),
346            Some("GAGLL") => self.update_gll(&parts, "GALILEO"),
347            Some("BDGLL") => self.update_gll(&parts, "BEIDOU"),
348            _ => {}
349        }
350    }
351
352    /// Calculates a fused position from all available GNSS systems using weighted averaging.
353    ///
354    /// The fused position is stored in `self.fused_position`.
355    ///
356    /// # Example
357    /// ```
358    /// use nema_parser::gnss_multignss_parser::GnssData;
359    /// let mut gnss = GnssData::new();
360    /// gnss.calculate_fused_position();
361    /// if let Some(fused) = &gnss.fused_position {
362    ///     println!("Fused position: {}, {}", fused.latitude, fused.longitude);
363    /// }
364    /// ```
365    pub fn calculate_fused_position(&mut self) {
366        let mut valid_positions = Vec::new();
367
368        for (system_name, system_data) in &self.systems {
369            if system_data.satellites_info.len() >= 4 {
370                if let (Some(lat), Some(lon), Some(hdop)) = (system_data.latitude, system_data.longitude, system_data.hdop) {
371                    let altitude = system_data.altitude.unwrap_or(0.0);
372                    let vdop = system_data.vdop.unwrap_or(hdop * 1.5); // Default VDOP if not available
373                    let system_accuracy = system_data.accuracy;
374                    valid_positions.push((system_name.to_string(), lat, lon, altitude, hdop, vdop, system_accuracy));
375                }
376            }
377        }
378
379        if valid_positions.is_empty() {
380            self.fused_position = None;
381            return;
382        }
383
384        if valid_positions.len() == 1 {
385            let (system, lat, lon, altitude, hdop, vdop, system_accuracy) = &valid_positions[0];
386            // Use system accuracy as multiplier instead of hardcoded 3.0
387            let horizontal_accuracy = (hdop * system_accuracy).max(*system_accuracy);
388            let vertical_accuracy = (vdop * system_accuracy * 1.5).max(*system_accuracy * 1.5);
389
390            self.fused_position = Some(FusedPosition {
391                latitude: *lat,
392                longitude: *lon,
393                altitude: *altitude,
394                estimated_accuracy: horizontal_accuracy,
395                altitude_accuracy: vertical_accuracy,
396                contributing_systems: vec![system.clone()],
397            });
398            return;
399        }
400
401        // Weighted average using inverse of combined accuracy (DOP + system accuracy) as weights
402        let mut weighted_lat = 0.0;
403        let mut weighted_lon = 0.0;
404        let mut weighted_alt = 0.0;
405        let mut total_weight = 0.0;
406        let mut total_alt_weight = 0.0;
407        let mut contributing_systems = Vec::new();
408
409        for (system, lat, lon, altitude, hdop, vdop, system_accuracy) in &valid_positions {
410            // Use system accuracy as the multiplier for DOP values instead of hardcoded constants
411            let combined_horizontal_accuracy = (hdop * system_accuracy).max(*system_accuracy);
412            let combined_vertical_accuracy = (vdop * system_accuracy * 1.5).max(*system_accuracy * 1.5);
413
414            let weight = 1.0 / (combined_horizontal_accuracy + 0.1); // Add small value to avoid division by zero
415            let alt_weight = 1.0 / (combined_vertical_accuracy + 0.1); // Weight for altitude
416
417            weighted_lat += lat * weight;
418            weighted_lon += lon * weight;
419            weighted_alt += altitude * alt_weight;
420            total_weight += weight;
421            total_alt_weight += alt_weight;
422            contributing_systems.push(system.clone());
423        }
424
425        if total_weight > 0.0 {
426            let fused_lat = weighted_lat / total_weight;
427            let fused_lon = weighted_lon / total_weight;
428            let fused_alt = if total_alt_weight > 0.0 { weighted_alt / total_alt_weight } else { 0.0 };
429
430            // Calculate fused accuracy based on weighted system accuracies and DOP values
431            let mut weighted_horizontal_accuracy = 0.0;
432            let mut weighted_vertical_accuracy = 0.0;
433            let mut total_weight = 0.0;
434            let mut total_alt_weight = 0.0;
435
436            for (_, _, _, _, hdop, vdop, system_accuracy) in &valid_positions {
437                // Use system accuracy as multiplier instead of hardcoded 2.0
438                let combined_horizontal_accuracy = (hdop * system_accuracy).max(*system_accuracy);
439                let combined_vertical_accuracy = (vdop * system_accuracy * 1.5).max(*system_accuracy * 1.5);
440
441                let weight = 1.0 / (combined_horizontal_accuracy + 0.1);
442                let alt_weight = 1.0 / (combined_vertical_accuracy + 0.1);
443
444                weighted_horizontal_accuracy += combined_horizontal_accuracy * weight;
445                weighted_vertical_accuracy += combined_vertical_accuracy * alt_weight;
446                total_weight += weight;
447                total_alt_weight += alt_weight;
448            }
449
450            let final_horizontal_accuracy = if total_weight > 0.0 {
451                (weighted_horizontal_accuracy / total_weight).max(self.get_fused_accuracy())
452            } else {
453                self.get_fused_accuracy()
454            };
455            let final_vertical_accuracy = if total_alt_weight > 0.0 {
456                (weighted_vertical_accuracy / total_alt_weight)
457                    .max(self.get_fused_accuracy() * 1.5)
458            } else {
459                final_horizontal_accuracy * 1.5
460            };
461
462            self.fused_position = Some(FusedPosition {
463                latitude: fused_lat,
464                longitude: fused_lon,
465                altitude: fused_alt,
466                estimated_accuracy: final_horizontal_accuracy,
467                altitude_accuracy: final_vertical_accuracy,
468                contributing_systems,
469            });
470        } else {
471            self.fused_position = None;
472        }
473    }
474
475    /// Calculates an advanced fused position using a Kalman-like filtering approach.
476    ///
477    /// The fused position is stored in `self.fused_position`.
478    pub fn calculate_advanced_fused_position(&mut self) {
479        let mut valid_positions = Vec::new();
480
481        for (system_name, system_data) in &self.systems {
482            if let (Some(lat), Some(lon), Some(hdop), Some(pdop)) = (system_data.latitude, system_data.longitude, system_data.hdop, system_data.pdop) {
483                let altitude = system_data.altitude.unwrap_or(0.0);
484                let vdop = system_data.vdop.unwrap_or(pdop * 0.8); // Default VDOP if not available
485                let system_accuracy = system_data.accuracy;
486                valid_positions.push((system_name.to_string(), lat, lon, altitude, hdop, pdop, vdop, system_accuracy));
487            }
488        }
489
490        if valid_positions.is_empty() {
491            self.fused_position = None;
492            return;
493        }
494
495        // Kalman-like filtering approach
496        let mut weighted_lat = 0.0;
497        let mut weighted_lon = 0.0;
498        let mut weighted_alt = 0.0;
499        let mut total_weight = 0.0;
500        let mut total_alt_weight = 0.0;
501        let mut contributing_systems = Vec::new();
502
503        for (system, lat, lon, altitude, hdop, pdop, vdop, system_accuracy) in &valid_positions {
504            // Combined weight using both HDOP, PDOP and system accuracy for horizontal accuracy
505            let combined_dop = (hdop * hdop + pdop * pdop).sqrt();
506            let combined_horizontal_accuracy = combined_dop.max(*system_accuracy);
507            let weight = 1.0 / (combined_horizontal_accuracy + 0.1);
508
509            // Weight for altitude based on VDOP, PDOP and system accuracy
510            let alt_combined_dop = (vdop * vdop + pdop * pdop).sqrt();
511            let combined_vertical_accuracy = alt_combined_dop.max(*system_accuracy * 1.5);
512            let alt_weight = 1.0 / (combined_vertical_accuracy + 0.1);
513
514            weighted_lat += lat * weight;
515            weighted_lon += lon * weight;
516            weighted_alt += altitude * alt_weight;
517            total_weight += weight;
518            total_alt_weight += alt_weight;
519            contributing_systems.push(system.clone());
520        }
521
522        if total_weight > 0.0 {
523            let fused_lat = weighted_lat / total_weight;
524            let fused_lon = weighted_lon / total_weight;
525            let fused_alt = if total_alt_weight > 0.0 { weighted_alt / total_alt_weight } else { 0.0 };
526
527            // Calculate confidence interval for horizontal accuracy using system accuracies
528            let variance: f64 = valid_positions.iter()
529                .map(|(_, lat, lon, _, hdop, _, _, system_accuracy)| {
530                    let combined_accuracy = hdop.max(*system_accuracy);
531                    let weight = 1.0 / (combined_accuracy + 0.1);
532                    let lat_diff = lat - fused_lat;
533                    let lon_diff = lon - fused_lon;
534                    weight * (lat_diff * lat_diff + lon_diff * lon_diff)
535                })
536                .sum::<f64>() / total_weight;
537
538            let estimated_accuracy = (variance.sqrt() * 111000.0).max(self.get_fused_accuracy()); // Convert to meters and apply minimum
539
540            // Calculate altitude variance and accuracy using system accuracies
541            let alt_variance: f64 = if total_alt_weight > 0.0 {
542                valid_positions.iter()
543                    .map(|(_, _, _, altitude, _, _, vdop, system_accuracy)| {
544                        let combined_accuracy = vdop.max(*system_accuracy * 1.5);
545                        let weight = 1.0 / (combined_accuracy + 0.1);
546                        let alt_diff = altitude - fused_alt;
547                        weight * (alt_diff * alt_diff)
548                    })
549                    .sum::<f64>() / total_alt_weight
550            } else {
551                0.0
552            };
553
554            let altitude_accuracy = if alt_variance > 0.0 {
555                alt_variance.sqrt().max(self.get_fused_accuracy() * 1.5) // Minimum based on fused accuracy
556            } else {
557                (estimated_accuracy * 1.5).max(self.get_fused_accuracy() * 1.5) // Default to 1.5x horizontal accuracy
558            };
559
560            self.fused_position = Some(FusedPosition {
561                latitude: fused_lat,
562                longitude: fused_lon,
563                altitude: fused_alt,
564                estimated_accuracy: estimated_accuracy.max(self.get_fused_accuracy()), // Apply minimum fused accuracy
565                altitude_accuracy,
566                contributing_systems,
567            });
568        } else {
569            self.fused_position = None;
570        }
571    }
572
573    /// Gets the fused data accuracy in meters.
574    ///
575    /// # Returns
576    /// * `f64` - The fused accuracy value in meters
577    ///
578    /// # Example
579    /// ```
580    /// use nema_parser::gnss_multignss_parser::GnssData;
581    /// let gnss = GnssData::new();
582    /// // The fused accuracy is calculated using RSS formula from active system accuracies
583    /// assert!((gnss.get_fused_accuracy() - 1.37).abs() < 0.01);
584    /// ```
585    pub fn get_fused_accuracy(&self) -> f64 {
586        let mut active_systems = Vec::new();
587        for system_data in self.systems.values() {
588            if !system_data.satellites_info.is_empty() &&
589               system_data.latitude.is_some() &&
590               system_data.longitude.is_some() {
591                active_systems.push(system_data.accuracy);
592            }
593        }
594        if active_systems.is_empty() {
595            // If no active systems, use all system dynamic accuracies
596            active_systems = self.systems.values().map(|sys| sys.accuracy).collect();
597        }
598        let sum_of_inverse_squares: f64 = active_systems.iter()
599            .map(|accuracy| 1.0 / accuracy.powi(2))
600            .sum();
601        if sum_of_inverse_squares == 0.0 {
602            return 0.0;
603        }
604        1.0 / sum_of_inverse_squares.sqrt()
605    }
606
607    /// Sets the fused data accuracy in meters.
608    ///
609    /// # Arguments
610    /// * `accuracy` - The fused accuracy value in meters
611    ///
612    /// # Example
613    /// ```
614    /// use nema_parser::gnss_multignss_parser::GnssData;
615    /// let mut gnss = GnssData::new();
616    /// gnss.set_fused_accuracy(3.0);
617    /// // Fused accuracy is always dynamically calculated, not set by this function
618    /// let fused_accuracy = gnss.get_fused_accuracy();
619    /// assert!((fused_accuracy - 1.3675).abs() < 0.01);
620    /// ```
621    pub fn set_fused_accuracy(&mut self, _accuracy: f64) {
622        // No-op: fused accuracy is now always dynamic
623    }
624
625    /// Gets the dynamic accuracy for a specific GNSS system in meters.
626    ///
627    /// # Arguments
628    /// * `system` - The GNSS system name ("GPS", "GLONASS", "GALILEO", "BEIDOU")
629    ///
630    /// # Returns
631    /// * `Option<f64>` - The system accuracy value in meters, or None if system doesn't exist
632    ///
633    /// # Example
634    /// ```
635    /// use nema_parser::gnss_multignss_parser::GnssData;
636    /// let gnss = GnssData::new();
637    /// assert_eq!(gnss.get_system_accuracy("GPS"), Some(2.0));
638    /// assert_eq!(gnss.get_system_accuracy("GLONASS"), Some(4.0));
639    /// ```
640    pub fn get_system_accuracy(&self, system: &str) -> Option<f64> {
641        self.systems.get(system).map(|sys| sys.accuracy)
642    }
643
644    /// Gets the fixed accuracy for a specific GNSS system in meters.
645    ///
646    /// # Arguments
647    /// * `system` - The GNSS system name ("GPS", "GLONASS", "GALILEO", "BEIDOU")
648    ///
649    /// # Returns
650    /// * `Option<f64>` - The fixed accuracy value in meters, or None if the system doesn't exist
651    ///
652    /// # Example
653    /// ```
654    /// use nema_parser::gnss_multignss_parser::GnssData;
655    /// let gnss = GnssData::new();
656    /// assert_eq!(gnss.get_system_fixed_accuracy("GPS"), Some(2.0));
657    /// assert_eq!(gnss.get_system_fixed_accuracy("GLONASS"), Some(4.0));
658    /// ```
659    pub fn get_system_fixed_accuracy(&self, system: &str) -> Option<f64> {
660        self.systems.get(system).map(|sys| sys.fixed_accuracy)
661    }
662
663    /// Sets the accuracy for a specific GNSS system in meters.
664    ///
665    /// # Arguments
666    /// * `system` - The GNSS system name ("GPS", "GLONASS", "GALILEO", "BEIDOU")
667    /// * `accuracy` - The system accuracy value in meters
668    ///
669    /// # Returns
670    /// * `bool` - True if the system exists and accuracy was set, false otherwise
671    ///
672    /// # Example
673    /// ```
674    /// use nema_parser::gnss_multignss_parser::GnssData;
675    /// let mut gnss = GnssData::new();
676    /// assert!(gnss.set_system_fixed_accuracy("GPS", 1.5));
677    /// assert_eq!(gnss.get_system_fixed_accuracy("GPS"), Some(1.5));
678    /// assert!(!gnss.set_system_fixed_accuracy("INVALID", 1.0));
679    /// ```
680    pub fn set_system_fixed_accuracy(&mut self, system: &str, accuracy: f64) -> bool {
681        if let Some(sys) = self.systems.get_mut(system) {
682            sys.fixed_accuracy = accuracy;
683            true
684        } else {
685            false
686        }
687    }
688
689    /// Gets all system accuracies as a HashMap.
690    ///
691    /// # Returns
692    /// * `HashMap<String, f64>` - Map of system names to their accuracy values in meters
693    ///
694    /// # Example
695    /// ```
696    /// use nema_parser::gnss_multignss_parser::GnssData;
697    /// let gnss = GnssData::new();
698    /// let accuracies = gnss.get_all_system_accuracies();
699    /// assert_eq!(accuracies.get("GPS"), Some(&2.0));
700    /// assert_eq!(accuracies.get("GLONASS"), Some(&4.0));
701    /// ```
702    pub fn get_all_system_accuracies(&self) -> HashMap<String, f64> {
703        self.systems.iter()
704            .map(|(name, sys)| (name.to_string(), sys.fixed_accuracy))
705            .collect()
706    }
707}
708
709/// Parses latitude from NMEA format to decimal degrees.
710///
711/// # Arguments
712/// * `value` - Latitude value as string (DDMM.MMMM)
713/// * `hemi` - Hemisphere ("N" or "S")
714///
715/// # Returns
716/// * `Option<f64>` - Latitude in decimal degrees
717fn parse_lat(value: Option<&&str>, hemi: Option<&&str>) -> Option<f64> {
718    let val = value?.parse::<f64>().ok()?;
719    let deg = (val / 100.0).floor();
720    let min = val % 100.0;
721    let mut result = deg + min / 60.0;
722    if hemi? == &"S" { result *= -1.0; }
723    Some(result)
724}
725
726/// Parses longitude from NMEA format to decimal degrees.
727///
728/// # Arguments
729/// * `value` - Longitude value as string (DDDMM.MMMM)
730/// * `hemi` - Hemisphere ("E" or "W")
731///
732/// # Returns
733/// * `Option<f64>` - Longitude in decimal degrees
734fn parse_lon(value: Option<&&str>, hemi: Option<&&str>) -> Option<f64> {
735    let val = value?.parse::<f64>().ok()?;
736    let deg = (val / 100.0).floor();
737    let min = val % 100.0;
738    let mut result = deg + min / 60.0;
739    if hemi? == &"W" { result *= -1.0; }
740    Some(result)
741}
742
743#[cfg(test)]
744mod tests {
745    use super::*;
746
747    #[test]
748    fn test_gnssdata_new() {
749        let gnss = GnssData::new();
750        assert!(gnss.systems.contains_key("GPS"));
751        assert!(gnss.systems.contains_key("GLONASS"));
752        assert!(gnss.systems.contains_key("GALILEO"));
753        assert!(gnss.systems.contains_key("BEIDOU"));
754    }
755
756    #[test]
757    fn test_feed_nmea_gga() {
758        let mut gnss = GnssData::new();
759        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
760        gnss.feed_nmea(gga);
761        assert_eq!(gnss.time, Some("123519".to_string()));
762        assert!(gnss.latitude.is_some());
763        assert!(gnss.longitude.is_some());
764        assert_eq!(gnss.fix_quality, Some(1));
765        assert_eq!(gnss.num_satellites, Some(8));
766        assert_eq!(gnss.altitude, Some(545.4));
767    }
768
769    #[test]
770    fn test_feed_nmea_gps_gsv() {
771        let mut gnss = GnssData::new();
772        let gsv = "$GPGSV,2,1,08,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*7C";
773        gnss.feed_nmea(gsv);
774        let gps_info = &gnss.systems["GPS"].satellites_info;
775        assert_eq!(gps_info.len(), 4);
776        assert!(gps_info.contains_key(&1));
777        assert!(gps_info.contains_key(&2));
778        assert!(gps_info.contains_key(&3));
779        assert!(gps_info.contains_key(&4));
780        let sat1 = gps_info.get(&1).unwrap();
781        assert_eq!(sat1.prn, 1);
782        assert_eq!(sat1.elevation, Some(40));
783        assert_eq!(sat1.azimuth, Some(83));
784        assert_eq!(sat1.snr, Some(41));
785    }
786
787    #[test]
788    fn test_feed_nmea_glonass_gsv() {
789        let mut gnss = GnssData::new();
790        let gsv = "$GLGSV,2,1,08,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*61";
791        gnss.feed_nmea(gsv);
792        let glonass_info = &gnss.systems["GLONASS"].satellites_info;
793        assert_eq!(glonass_info.len(), 4);
794        assert!(glonass_info.contains_key(&67));
795        assert!(glonass_info.contains_key(&68));
796        assert!(glonass_info.contains_key(&69));
797        assert!(glonass_info.contains_key(&77));
798        let sat67 = glonass_info.get(&67).unwrap();
799        assert_eq!(sat67.prn, 67);
800        assert_eq!(sat67.elevation, Some(14));
801        assert_eq!(sat67.azimuth, Some(186));
802        assert_eq!(sat67.snr, Some(9));
803    }
804
805    #[test]
806    fn test_feed_nmea_galileo_gsv() {
807        let mut gnss = GnssData::new();
808        let gsv = "$GAGSV,1,1,04,301,45,123,35,302,30,045,40,303,60,234,45,304,25,156,38*XX";
809        gnss.feed_nmea(gsv);
810        let galileo_info = &gnss.systems["GALILEO"].satellites_info;
811        assert_eq!(galileo_info.len(), 4);
812        assert!(galileo_info.contains_key(&301));
813        assert!(galileo_info.contains_key(&302));
814        assert!(galileo_info.contains_key(&303));
815        assert!(galileo_info.contains_key(&304));
816        let sat301 = galileo_info.get(&301).unwrap();
817        assert_eq!(sat301.prn, 301);
818        assert_eq!(sat301.elevation, Some(45));
819        assert_eq!(sat301.azimuth, Some(123));
820        assert_eq!(sat301.snr, Some(35));
821    }
822
823    #[test]
824    fn test_feed_nmea_beidou_gsv() {
825        let mut gnss = GnssData::new();
826        let gsv = "$BDGSV,1,1,04,201,45,123,35,202,30,045,40,203,60,234,45,204,25,156,38*XX";
827        gnss.feed_nmea(gsv);
828        let beidou_info = &gnss.systems["BEIDOU"].satellites_info;
829        assert_eq!(beidou_info.len(), 4);
830        assert!(beidou_info.contains_key(&201));
831        assert!(beidou_info.contains_key(&202));
832        assert!(beidou_info.contains_key(&203));
833        assert!(beidou_info.contains_key(&204));
834        let sat201 = beidou_info.get(&201).unwrap();
835        assert_eq!(sat201.prn, 201);
836        assert_eq!(sat201.elevation, Some(45));
837        assert_eq!(sat201.azimuth, Some(123));
838        assert_eq!(sat201.snr, Some(35));
839    }
840
841    #[test]
842    fn test_feed_nmea_gsa_gps() {
843        let mut gnss = GnssData::new();
844        let gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,09,10,11,12,1.2,0.9,2.1*39";
845        gnss.feed_nmea(gsa);
846        let gps_used = &gnss.systems["GPS"].satellites_used;
847        assert!(gps_used.contains(&1));
848        assert!(gps_used.contains(&2));
849        assert!(gps_used.contains(&3));
850        assert!(gps_used.contains(&4));
851        assert_eq!(gnss.systems["GPS"].pdop, Some(1.2));
852        assert_eq!(gnss.systems["GPS"].hdop, Some(0.9));
853        assert_eq!(gnss.systems["GPS"].vdop, Some(2.1));
854    }
855
856    #[test]
857    fn test_feed_nmea_gsa_glonass() {
858        let mut gnss = GnssData::new();
859        let gsa = "$GNGSA,A,3,67,68,69,77,78,79,86,87,88,,,,,1.8,1.1,1.4*3F";
860        gnss.feed_nmea(gsa);
861
862        // Debug output
863        println!("GLONASS satellites_used: {:?}", gnss.systems["GLONASS"].satellites_used);
864        println!("GLONASS pdop: {:?}", gnss.systems["GLONASS"].pdop);
865        println!("GLONASS hdop: {:?}", gnss.systems["GLONASS"].hdop);
866        println!("GLONASS vdop: {:?}", gnss.systems["GLONASS"].vdop);
867
868        let glonass_used = &gnss.systems["GLONASS"].satellites_used;
869        assert!(glonass_used.contains(&67));
870        assert!(glonass_used.contains(&68));
871        assert!(glonass_used.contains(&69));
872        assert!(glonass_used.contains(&77));
873        assert_eq!(gnss.systems["GLONASS"].pdop, Some(1.8));
874        assert_eq!(gnss.systems["GLONASS"].hdop, Some(1.1));
875        assert_eq!(gnss.systems["GLONASS"].vdop, Some(1.4));
876    }
877
878    #[test]
879    fn test_feed_nmea_gsa_galileo() {
880        let mut gnss = GnssData::new();
881        let gsa = "$GNGSA,A,3,301,302,303,304,305,306,,,,,,,2.1,1.3,1.6*XX";
882        gnss.feed_nmea(gsa);
883        let galileo_used = &gnss.systems["GALILEO"].satellites_used;
884        assert!(galileo_used.contains(&301));
885        assert!(galileo_used.contains(&302));
886        assert!(galileo_used.contains(&303));
887        assert!(galileo_used.contains(&304));
888        assert_eq!(gnss.systems["GALILEO"].pdop, Some(2.1));
889        assert_eq!(gnss.systems["GALILEO"].hdop, Some(1.3));
890        assert_eq!(gnss.systems["GALILEO"].vdop, Some(1.6));
891    }
892
893    #[test]
894    fn test_feed_nmea_gsa_beidou() {
895        let mut gnss = GnssData::new();
896        let gsa = "$GNGSA,A,3,201,202,203,204,205,206,,,,,,,1.5,0.8,1.2*XX";
897        gnss.feed_nmea(gsa);
898        let beidou_used = &gnss.systems["BEIDOU"].satellites_used;
899        assert!(beidou_used.contains(&201));
900        assert!(beidou_used.contains(&202));
901        assert!(beidou_used.contains(&203));
902        assert!(beidou_used.contains(&204));
903        assert_eq!(gnss.systems["BEIDOU"].pdop, Some(1.5));
904        assert_eq!(gnss.systems["BEIDOU"].hdop, Some(0.8));
905        assert_eq!(gnss.systems["BEIDOU"].vdop, Some(1.2));
906    }
907
908    #[test]
909    fn test_coordinates_update_for_systems_with_satellites() {
910        let mut gnss = GnssData::new();
911
912        // Add GPS satellites
913        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
914        gnss.feed_nmea(gps_gsv);
915
916        // Add GLONASS satellites
917        let glonass_gsv = "$GLGSV,1,1,04,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*XX";
918        gnss.feed_nmea(glonass_gsv);
919
920        // Update coordinates via GGA
921        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
922        gnss.feed_nmea(gga);
923
924        // Systems with satellites should have coordinates
925        assert!(gnss.systems["GPS"].latitude.is_some());
926        assert!(gnss.systems["GPS"].longitude.is_some());
927        assert!(gnss.systems["GLONASS"].latitude.is_some());
928        assert!(gnss.systems["GLONASS"].longitude.is_some());
929
930        // Systems without satellites should not have coordinates
931        assert!(gnss.systems["GALILEO"].latitude.is_none());
932        assert!(gnss.systems["GALILEO"].longitude.is_none());
933        assert!(gnss.systems["BEIDOU"].latitude.is_none());
934        assert!(gnss.systems["BEIDOU"].longitude.is_none());
935    }
936
937    #[test]
938    fn test_fused_position_calculation() {
939        let mut gnss = GnssData::new();
940
941        // Add GPS satellites and coordinates
942        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
943        gnss.feed_nmea(gps_gsv);
944        let gps_gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,,,,,1.2,0.9,2.1*39";
945        gnss.feed_nmea(gps_gsa);
946
947        // Add GLONASS satellites and coordinates
948        let glonass_gsv = "$GLGSV,1,1,04,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*XX";
949        gnss.feed_nmea(glonass_gsv);
950        let glonass_gsa = "$GNGSA,A,3,67,68,69,77,78,79,86,87,,,,,1.8,1.1,1.4*3F";
951        gnss.feed_nmea(glonass_gsa);
952
953        // Update coordinates
954        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
955        gnss.feed_nmea(gga);
956
957        // Calculate fused position
958        gnss.calculate_fused_position();
959
960        assert!(gnss.fused_position.is_some());
961        let fused = gnss.fused_position.as_ref().unwrap();
962        assert!(fused.contributing_systems.contains(&"GPS".to_string()));
963        assert!(fused.contributing_systems.contains(&"GLONASS".to_string()));
964        assert!(fused.estimated_accuracy > 0.0);
965    }
966
967    #[test]
968    fn test_fused_position_with_altitude() {
969        let mut gnss = GnssData::new();
970
971        // Add GPS satellites and coordinates with altitude
972        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
973        gnss.feed_nmea(gps_gsv);
974        let gps_gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,,,,,1.2,0.9,2.1*39";
975        gnss.feed_nmea(gps_gsa);
976
977        // Add GLONASS satellites and coordinates
978        let glonass_gsv = "$GLGSV,1,1,04,67,14,186,09,68,49,228,26,69,42,308,,77,15,064,17*XX";
979        gnss.feed_nmea(glonass_gsv);
980        let glonass_gsa = "$GNGSA,A,3,67,68,69,77,78,79,86,87,,,,,1.8,1.1,1.4*3F";
981        gnss.feed_nmea(glonass_gsa);
982
983        // Update coordinates with altitude data
984        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
985        gnss.feed_nmea(gga);
986
987        // Verify altitude is stored in system data
988        assert_eq!(gnss.systems["GPS"].altitude, Some(545.4));
989        assert_eq!(gnss.systems["GLONASS"].altitude, Some(545.4));
990
991        // Calculate fused position
992        gnss.calculate_fused_position();
993
994        assert!(gnss.fused_position.is_some());
995        let fused = gnss.fused_position.as_ref().unwrap();
996
997        // Verify altitude and altitude accuracy are calculated (use approximate comparison for floating point)
998        assert!((fused.altitude - 545.4).abs() < 0.001);
999        assert!(fused.altitude_accuracy > 0.0);
1000        assert!(fused.estimated_accuracy > 0.0);
1001
1002        // Verify contributing systems
1003        assert!(fused.contributing_systems.contains(&"GPS".to_string()));
1004        assert!(fused.contributing_systems.contains(&"GLONASS".to_string()));
1005    }
1006
1007    #[test]
1008    fn test_advanced_fused_position_with_altitude() {
1009        let mut gnss = GnssData::new();
1010
1011        // Add GPS satellites and coordinates
1012        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
1013        gnss.feed_nmea(gps_gsv);
1014        let gps_gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,,,,,1.2,0.9,2.1*39";
1015        gnss.feed_nmea(gps_gsa);
1016
1017        // Add GALILEO satellites with different DOP values
1018        let galileo_gsv = "$GAGSV,1,1,04,301,45,123,35,302,30,045,40,303,60,234,45,304,25,156,38*XX";
1019        gnss.feed_nmea(galileo_gsv);
1020        let galileo_gsa = "$GNGSA,A,3,301,302,303,304,305,306,,,,,,,2.1,1.3,1.6*XX";
1021        gnss.feed_nmea(galileo_gsa);
1022
1023        // Update coordinates with altitude
1024        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
1025        gnss.feed_nmea(gga);
1026
1027        // Calculate advanced fused position
1028        gnss.calculate_advanced_fused_position();
1029
1030        assert!(gnss.fused_position.is_some());
1031        let fused = gnss.fused_position.as_ref().unwrap();
1032
1033        // Verify altitude fusion and accuracy calculation
1034        assert!((fused.altitude - 545.4).abs() < 0.001);
1035        // Since both systems have identical altitude, variance will be 0, so altitude_accuracy will be 1.5x horizontal accuracy
1036        assert!(fused.altitude_accuracy > 0.0); // Just ensure it's positive
1037        assert!(fused.estimated_accuracy >= 1.0); // Minimum 1 meter accuracy
1038
1039        // Should have both GPS and GALILEO contributing
1040        assert!(fused.contributing_systems.contains(&"GPS".to_string()));
1041        assert!(fused.contributing_systems.contains(&"GALILEO".to_string()));
1042    }
1043
1044    #[test]
1045    fn test_parse_lat_lon() {
1046        let lat = parse_lat(Some(&"4807.038"), Some(&"N"));
1047        let lon = parse_lon(Some(&"01131.000"), Some(&"E"));
1048        assert!(lat.is_some());
1049        assert!(lon.is_some());
1050        let lat_val = lat.unwrap();
1051        let lon_val = lon.unwrap();
1052        assert!((lat_val - 48.1173).abs() < 0.0001);
1053        assert!((lon_val - 11.5166667).abs() < 0.0001);
1054    }
1055
1056    #[test]
1057    fn test_beidou_altitude_integration() {
1058        let mut gnss = GnssData::new();
1059
1060        // Add BeiDou satellites
1061        let beidou_gsv = "$BDGSV,1,1,04,201,45,123,35,202,30,045,40,203,60,234,45,204,25,156,38*XX";
1062        gnss.feed_nmea(beidou_gsv);
1063        let beidou_gsa = "$GNGSA,A,3,201,202,203,204,205,206,,,,,,,1.5,0.8,1.2*XX";
1064        gnss.feed_nmea(beidou_gsa);
1065
1066        // Add GPS for comparison
1067        let gps_gsv = "$GPGSV,1,1,04,01,40,083,41,02,17,308,43,03,13,172,42,04,09,020,39*XX";
1068        gnss.feed_nmea(gps_gsv);
1069        let gps_gsa = "$GNGSA,A,3,01,02,03,04,05,06,07,08,,,,,1.2,0.9,2.1*39";
1070        gnss.feed_nmea(gps_gsa);
1071
1072        // Update coordinates with altitude
1073        let gga = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,445.2,M,46.9,M,,*47";
1074        gnss.feed_nmea(gga);
1075
1076        // Verify BeiDou has altitude data
1077        assert_eq!(gnss.systems["BEIDOU"].altitude, Some(445.2));
1078        assert_eq!(gnss.systems["GPS"].altitude, Some(445.2));
1079
1080        // Calculate fused position including BeiDou
1081        gnss.calculate_fused_position();
1082
1083        assert!(gnss.fused_position.is_some());
1084        let fused = gnss.fused_position.as_ref().unwrap();
1085
1086        // Verify BeiDou contributes to altitude fusion
1087        assert!((fused.altitude - 445.2).abs() < 0.001);
1088        assert!(fused.altitude_accuracy > 0.0);
1089        assert!(fused.contributing_systems.contains(&"BEIDOU".to_string()));
1090        assert!(fused.contributing_systems.contains(&"GPS".to_string()));
1091    }
1092
1093    #[test]
1094    fn test_default_accuracy_values() {
1095        let gnss = GnssData::new();
1096        
1097        // Calculate expected fused accuracy using RSS formula
1098        let expected_fused_accuracy = 1_f64/(((1_f64/2.0_f64.powi(2)) +
1099                                              (1_f64/4.0_f64.powi(2)) +
1100                                              (1_f64/3.0_f64.powi(2)) +
1101                                              (1_f64/3.0_f64.powi(2))).sqrt());
1102
1103        // Test calculated fused accuracy (approximately 1.37)
1104        assert!((gnss.get_fused_accuracy() - expected_fused_accuracy).abs() < 0.01);
1105
1106        // Test default system accuracies
1107        assert_eq!(gnss.get_system_accuracy("GPS"), Some(2.0));
1108        assert_eq!(gnss.get_system_accuracy("GLONASS"), Some(4.0));
1109        assert_eq!(gnss.get_system_accuracy("GALILEO"), Some(3.0));
1110        assert_eq!(gnss.get_system_accuracy("BEIDOU"), Some(3.0));
1111        assert_eq!(gnss.get_system_accuracy("INVALID"), None);
1112    }
1113
1114    #[test]
1115    fn test_accuracy_getters_and_setters() {
1116        let mut gnss = GnssData::new();
1117
1118        // Calculate expected fused accuracy using RSS formula
1119        let expected_fused_accuracy = 1_f64/(((1_f64/2.0_f64.powi(2)) +
1120                                              (1_f64/4.0_f64.powi(2)) +
1121                                              (1_f64/3.0_f64.powi(2)) +
1122                                              (1_f64/3.0_f64.powi(2))).sqrt());
1123
1124        // Test fused accuracy getter with calculated value (approximately 1.37)
1125        assert!((gnss.get_fused_accuracy() - expected_fused_accuracy).abs() < 0.01);
1126        gnss.set_fused_accuracy(3.0);
1127        // Fused accuracy is always dynamic, so check the value again
1128        assert!((gnss.get_fused_accuracy() - expected_fused_accuracy).abs() < 0.01);
1129
1130        // Test system accuracy getter and setter
1131        assert_eq!(gnss.get_system_fixed_accuracy("GPS"), Some(2.0));
1132        assert!(gnss.set_system_fixed_accuracy("GPS", 1.5));
1133        assert_eq!(gnss.get_system_fixed_accuracy("GPS"), Some(1.5));
1134
1135        // Test setting accuracy for invalid system
1136        assert!(!gnss.set_system_fixed_accuracy("INVALID", 1.0));
1137        assert_eq!(gnss.get_system_fixed_accuracy("INVALID"), None);
1138    }
1139
1140    #[test]
1141    fn test_get_all_system_accuracies() {
1142        let mut gnss = GnssData::new();
1143
1144        // Test getting all default accuracies
1145        let accuracies = gnss.get_all_system_accuracies();
1146        assert_eq!(accuracies.len(), 4);
1147        assert_eq!(accuracies.get("GPS"), Some(&2.0));
1148        assert_eq!(accuracies.get("GLONASS"), Some(&4.0));
1149        assert_eq!(accuracies.get("GALILEO"), Some(&3.0));
1150        assert_eq!(accuracies.get("BEIDOU"), Some(&3.0));
1151
1152        // Test after modifying some accuracies
1153        gnss.set_system_fixed_accuracy("GPS", 1.8);
1154        gnss.set_system_fixed_accuracy("GALILEO", 2.5);
1155
1156        let updated_accuracies = gnss.get_all_system_accuracies();
1157        assert_eq!(updated_accuracies.get("GPS"), Some(&1.8));
1158        assert_eq!(updated_accuracies.get("GLONASS"), Some(&4.0));
1159        assert_eq!(updated_accuracies.get("GALILEO"), Some(&2.5));
1160        assert_eq!(updated_accuracies.get("BEIDOU"), Some(&3.0));
1161    }
1162}
nema_parser/gnss_multignss_parser.rs

nema_parser/
gnss_multignss_parser.rs
