r_d_inclita_sofware/sensors/
gps.rs

1use crate::telemetry::data::{
2    DATA_CHANNEL, GnggaMessage, GpsFix, LATEST_TELEMETRY, LogEvent, UtcTime,
3};
4use defmt::{error, info, warn};
5use embassy_stm32::usart::Uart;
6
7#[embassy_executor::task]
8pub async fn gps_task(mut uart: Uart<'static, embassy_stm32::mode::Async>) {
9    // (Usually ~300-500 bytes
10    let mut dma_buf = [0u8; 400];
11
12    loop {
13        // The CPU halts this task entirely. DMA fills the buffer.
14        // It ONLY wakes up when the GPS finishes its sentence burst!
15        match uart.read_until_idle(&mut dma_buf).await {
16            Ok(bytes_read) => {
17                //PROCESS THE COMPLETE BURST
18                if let Ok(burst_str) = core::str::from_utf8(&dma_buf[..bytes_read]) {
19                    // Rust's .lines() automatically splits the burst by '\n'
20                    for line in burst_str.lines() {
21                        if line.starts_with("$GNGGA") || line.starts_with("$GPGGA") {
22                            parse_and_send_gngga(line).await;
23                        }
24                    }
25                }
26            }
27            Err(_) => error!("GPS UART Read Error"),
28        }
29    }
30}
31
32async fn parse_and_send_gngga(line: &str) {
33    let get_part = |idx: usize| -> Option<&str> { line.split(',').nth(idx) };
34
35    // Parse the raw number, then safely convert it to our Enum
36    let raw_fix_num = get_part(6).unwrap_or("0").parse::<u8>().unwrap_or(0);
37    let current_fix = GpsFix::from_u8(raw_fix_num);
38
39    // Check our enum
40    if current_fix != GpsFix::NoFix && current_fix != GpsFix::Unknown {
41        let raw_time = get_part(1).unwrap_or("000000.00");
42        let utc_time = parse_utc_time(raw_time);
43        let raw_lat = get_part(2).unwrap_or("0.0");
44        let lat_dir = get_part(3).unwrap_or("N");
45        let raw_lon = get_part(4).unwrap_or("0.0");
46        let lon_dir = get_part(5).unwrap_or("E");
47        let altitude = get_part(9).unwrap_or("0.0").parse::<f32>().unwrap_or(0.0);
48
49        let latitude = nmea_to_decimal(raw_lat, lat_dir);
50        let longitude = nmea_to_decimal(raw_lon, lon_dir);
51
52        let data = GnggaMessage {
53            utc_time,
54            latitude,
55            longitude,
56            altitude,
57            fix: current_fix,
58            timestamp_ms: embassy_time::Instant::now().as_millis() as u32,
59        };
60        info!("GPS {:?}", data);
61
62        DATA_CHANNEL.send(LogEvent::GPS(data.clone())).await;
63        {
64            let mut guard = LATEST_TELEMETRY.lock().await;
65
66            guard.gps = Some(data.into());
67        }
68    } else {
69        warn!("GPS: Waiting for Satellites (No Fix)");
70    }
71}
72/// Converts NMEA DDMM.MMMM format into standard Decimal Degrees
73fn nmea_to_decimal(raw: &str, direction: &str) -> f64 {
74    // should use match instead!
75    let val = match raw.parse::<f64>() {
76        Ok(v) => v,
77        Err(_) => return 0.0, // If parsing fails, return 0.0
78    };
79
80    // should be: if parsing fails, return none!
81    // ideally, it should use .parse() / .try_parse() function! its most ideomatic do to it this way!
82    // great blogs! https://www.howtocodeit.com/guides; you should read their NewTypes guide:
83    // https://www.howtocodeit.com/guides/ultimate-guide-rust-newtypes
84
85    // Extract Degrees (DD) and Minutes (MM.MMMM)
86    let degrees = (val / 100.0) as i32 as f64;
87    let minutes = val - (degrees * 100.0);
88
89    let mut decimal = degrees + (minutes / 60.0);
90
91    // South and West are negative coordinates
92    if direction == "S" || direction == "W" {
93        decimal = -decimal;
94    }
95
96    decimal
97}
98
99// parse UTCTime
100fn parse_utc_time(raw: &str) -> UtcTime {
101    // If the string is empty or corrupted, return zeros
102
103    // Slice the string by index: HH(0..2) MM(2..4) SS.SS(4..)
104    let hours = raw
105        .get(0..2)
106        .and_then(|c| c.parse::<u8>().ok())
107        .unwrap_or(0);
108    let minutes = raw
109        .get(2..4)
110        .and_then(|c| c.parse::<u8>().ok())
111        .unwrap_or(0);
112    let seconds = raw
113        .get(4..)
114        .and_then(|c| c.parse::<f32>().ok())
115        .unwrap_or(0.0);
116
117    UtcTime {
118        hours,
119        minutes,
120        seconds,
121    }
122}
r_d_inclita_sofware/sensors/gps.rs

r_d_inclita_sofware/sensors/
gps.rs
