From f6fc0a71c6c8d1881590b6b62b3d8db982a0b559 Mon Sep 17 00:00:00 2001 From: Richard Patching Date: Wed, 18 Jun 2025 08:35:48 +0100 Subject: [PATCH] Handle malformed NMEA sentences --- src/main.rs | 216 +++++++++++++++++++++++++++++++++++++++------------- 1 file changed, 163 insertions(+), 53 deletions(-) diff --git a/src/main.rs b/src/main.rs index 5413086..f357ae8 100644 --- a/src/main.rs +++ b/src/main.rs @@ -11,6 +11,91 @@ const APP_VERSION: &str = env!("CARGO_PKG_VERSION"); const GPS_BAUD: u32 = 9600; const INIT_DELAY_MS: u32 = 5000; // 5 second initialization delay const POWER_CHECK_INTERVAL: u32 = 1000; // Check power every second +const UART_BUFFER_SIZE: usize = 256; // Increased buffer size +const READ_DELAY_MS: u32 = 10; // Small delay between reads + +fn decimal_to_dms(decimal: f32) -> (i32, i32, f32) { + let degrees = decimal.abs() as i32; + let minutes_decimal = (decimal.abs() - degrees as f32) * 60.0; + let minutes = minutes_decimal as i32; + let seconds = (minutes_decimal - minutes as f32) * 60.0; + (degrees, minutes, seconds) +} + +fn format_coordinates(lat: f32, long: f32) -> String { + // Decimal degrees (DD) + let dd = format!("Decimal degrees (DD): {:.5}, {:.5}", lat, long); + + // Degrees, minutes and seconds (DMS) + let (lat_deg, lat_min, lat_sec) = decimal_to_dms(lat); + let (long_deg, long_min, long_sec) = decimal_to_dms(long); + let dms = format!( + "Degrees, minutes and seconds (DMS): {}°{}'{:.1}\"{}, {}°{}'{:.1}\"{}", + lat_deg, + lat_min, + lat_sec, + if lat >= 0.0 { "N" } else { "S" }, + long_deg, + long_min, + long_sec, + if long >= 0.0 { "E" } else { "W" } + ); + + // Degrees and decimal minutes (DMM) + let dmm = format!( + "Degrees and decimal minutes (DMM): {} {:.4}, {} {:.4}", + lat_deg, + (lat.abs() - lat_deg as f32) * 60.0, + long_deg, + (long.abs() - long_deg as f32) * 60.0 + ); + + format!("{}\n{}\n{}", dd, dms, dmm) +} + +fn parse_gll(sentence: &str) -> Option<(f32, f32)> { + let parts: Vec<&str> = sentence.split(',').collect(); + if parts.len() < 7 { + return None; + } + + // Parse latitude (DDMM.MMMM format) + if let (Ok(lat_deg), Ok(lat_min)) = (parts[1][..2].parse::(), parts[1][2..].parse::()) + { + let lat = lat_deg + lat_min / 60.0; + let lat = if parts[2] == "S" { -lat } else { lat }; + + // Parse longitude (DDDMM.MMMM format) + if let (Ok(long_deg), Ok(long_min)) = + (parts[3][..3].parse::(), parts[3][3..].parse::()) + { + let long = long_deg + long_min / 60.0; + let long = if parts[4] == "W" { -long } else { long }; + + return Some((lat, long)); + } + } + None +} + +fn is_valid_nmea_sentence(sentence: &str) -> bool { + // Check if sentence starts with $ and ends with * followed by checksum + if !sentence.starts_with('$') || !sentence.contains('*') { + return false; + } + + // Check if sentence has at least 6 characters (minimum valid NMEA length) + if sentence.len() < 6 { + return false; + } + + // Check if sentence has at least one comma (all NMEA sentences have fields) + if !sentence.contains(',') { + return false; + } + + true +} fn main() -> Result<(), Box> { // Initialize ESP-IDF @@ -25,20 +110,20 @@ fn main() -> Result<(), Box> { info!("3. GPS VCC → 3.3V"); info!("4. GPS GND → GND"); info!("----------------------------------------"); - info!("Please check if the red LED on the GPS module is lit"); + info!("Please check if the blue LED on the GPS module is blinking"); info!("This indicates the module is receiving power"); info!("----------------------------------------"); // Initialize peripherals let peripherals = Peripherals::take().unwrap(); - // Configure UART + // Configure UART with specific settings let config = UartConfig::default().baudrate(Hertz(GPS_BAUD)); info!("Initializing UART with config: baudrate={}", GPS_BAUD); - // Create UART driver - let mut uart_driver = UartDriver::new( + // Create UART driver with explicit pin configuration + let uart_driver = UartDriver::new( peripherals.uart2, peripherals.pins.gpio17, // TX peripherals.pins.gpio16, // RX @@ -55,10 +140,12 @@ fn main() -> Result<(), Box> { FreeRtos::delay_ms(INIT_DELAY_MS); // Buffer for GPS data - let mut buffer = [0u8; 128]; + let mut buffer = [0u8; UART_BUFFER_SIZE]; let mut timeout_count = 0; let mut last_data_time = 0; - let mut start_time = 0; + let start_time = 0; + let mut partial_sentence = String::new(); + let mut last_position: Option<(f32, f32)> = None; loop { let current_time = last_data_time + 1; @@ -68,44 +155,81 @@ fn main() -> Result<(), Box> { Ok(bytes_read) if bytes_read > 0 => { timeout_count = 0; // Reset timeout counter on successful read last_data_time = current_time; - let gps_data = String::from_utf8_lossy(&buffer[..bytes_read]); - info!("Raw GPS Data: {}", gps_data); - // Try to parse each line of GPS data - for line in gps_data.lines() { - if line.starts_with("$GPGGA") { - let mut sentence = line.to_string(); - match parse_gga(&mut sentence) { - Ok(position) => { - info!("Successfully parsed GPS data:"); - info!( - " Latitude: {} {}", - position.lat.abs(), - if position.lat >= 0.0 { "N" } else { "S" } - ); - info!( - " Longitude: {} {}", - position.long.abs(), - if position.long >= 0.0 { "E" } else { "W" } - ); - info!(" Altitude: {:.1} meters", position.alt); + // Convert received bytes to string, handling invalid UTF-8 + let new_data = String::from_utf8_lossy(&buffer[..bytes_read]); + info!("Raw data received ({} bytes): {}", bytes_read, new_data); + + // Add new data to partial sentence + partial_sentence.push_str(&new_data); + + // Process complete sentences + while let Some(end_idx) = partial_sentence.find('\n') { + let sentence = partial_sentence[..end_idx].trim().to_string(); + partial_sentence = partial_sentence[end_idx + 1..].to_string(); + + // Only process valid NMEA sentences + if is_valid_nmea_sentence(&sentence) { + info!("Complete sentence: {}", sentence); + + if sentence.starts_with("$GPGGA") { + info!("Attempting to parse GGA sentence"); + match parse_gga(&mut sentence.clone()) { + Ok(position) => { + info!("Successfully parsed GPS data from GGA:"); + info!("{}", format_coordinates(position.lat, position.long)); + info!(" Altitude: {:.1} meters", position.alt); + last_position = Some((position.lat, position.long)); + } + Err(e) => { + info!("Error parsing GGA sentence:"); + match e { + GpsError::InvalidMessageType => { + info!(" Invalid message type, expected GGA") + } + GpsError::InvalidLength => { + info!(" Invalid GGA sentence length") + } + GpsError::ParseError(msg) => { + info!(" Parse error: {}", msg) + } + GpsError::Other(msg) => info!(" Other error: {}", msg), + } + } } - Err(e) => { - info!("Error parsing GPS data:"); - match e { - GpsError::InvalidMessageType => { - info!(" Invalid message type, expected GGA") - } - GpsError::InvalidLength => { - info!(" Invalid GGA sentence length") - } - GpsError::ParseError(msg) => info!(" Parse error: {}", msg), - GpsError::Other(msg) => info!(" Other error: {}", msg), + } else if sentence.starts_with("$GPGLL") { + if let Some((lat, long)) = parse_gll(&sentence) { + info!("Successfully parsed GPS data from GLL:"); + info!("{}", format_coordinates(lat, long)); + last_position = Some((lat, long)); + } + } else if sentence.starts_with("$GPRMC") { + // Parse RMC for speed and course + let parts: Vec<&str> = sentence.split(',').collect(); + if parts.len() >= 13 && parts[2] == "A" { + if let (Ok(speed), Ok(course)) = + (parts[7].parse::(), parts[8].parse::()) + { + info!(" Speed: {:.1} knots", speed); + info!(" Course: {:.1}°", course); } } } } } + + // If partial sentence is getting too long, clear it + if partial_sentence.len() > 1024 { + info!("Partial sentence too long, clearing buffer"); + partial_sentence.clear(); + } + + if last_position != None { + info!("Last position: {:?}", last_position); + } + + // Add a small delay between reads to prevent buffer overflow + FreeRtos::delay_ms(READ_DELAY_MS); } Ok(_) => { timeout_count += 1; @@ -113,7 +237,7 @@ fn main() -> Result<(), Box> { // Only log every 10 timeouts to avoid spam info!("No GPS data received (timeout #{})", timeout_count); info!("Please check:"); - info!("1. GPS module is powered (3.3V) - red LED should be lit"); + info!("1. GPS module is powered (3.3V) - blue LED should be blinking"); info!("2. GPS TX is connected to ESP32 GPIO16 D16"); info!("3. GPS RX is connected to ESP32 GPIO17 D17"); info!("4. GPS GND is connected to ESP32 GND"); @@ -124,21 +248,7 @@ fn main() -> Result<(), Box> { } } - // Check if we've received any data in the last second - if current_time - last_data_time > POWER_CHECK_INTERVAL { - info!("GPS Module Status:"); - info!("1. Power LED should be lit (red)"); - info!( - "2. No data received for {} seconds", - (current_time - last_data_time) / 1000 - ); - info!( - "3. Total runtime: {} seconds", - (current_time - start_time) / 1000 - ); - } - info!("----------------------------------------"); - FreeRtos::delay_ms(3000); // Wait 3 seconds before next reading + FreeRtos::delay_ms(1000); // Wait 1 second before next reading } }