Handle malformed NMEA sentences

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::<f32>(), parts[1][2..].parse::<f32>())
+    {
+        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::<f32>(), parts[3][3..].parse::<f32>())
+        {
+            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<dyn std::error::Error>> {
     // Initialize ESP-IDF
@@ -25,20 +110,20 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     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
+    // Create UART driver with explicit pin configuration
-    let mut uart_driver = UartDriver::new(
+    let uart_driver = UartDriver::new(
         peripherals.uart2,
         peripherals.pins.gpio17,                  // TX
         peripherals.pins.gpio16,                  // RX
@@ -55,10 +140,12 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     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<dyn std::error::Error>> {
             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
+                // Convert received bytes to string, handling invalid UTF-8
-                for line in gps_data.lines() {
+                let new_data = String::from_utf8_lossy(&buffer[..bytes_read]);
-                    if line.starts_with("$GPGGA") {
+                info!("Raw data received ({} bytes): {}", bytes_read, new_data);
-                        let mut sentence = line.to_string();
+
-                        match parse_gga(&mut sentence) {
+                // Add new data to partial sentence
-                            Ok(position) => {
+                partial_sentence.push_str(&new_data);
-                                info!("Successfully parsed GPS data:");
+
-                                info!(
+                // Process complete sentences
-                                    "  Latitude: {} {}",
+                while let Some(end_idx) = partial_sentence.find('\n') {
-                                    position.lat.abs(),
+                    let sentence = partial_sentence[..end_idx].trim().to_string();
-                                    if position.lat >= 0.0 { "N" } else { "S" }
+                    partial_sentence = partial_sentence[end_idx + 1..].to_string();
-                                );
+
-                                info!(
+                    // Only process valid NMEA sentences
-                                    "  Longitude: {} {}",
+                    if is_valid_nmea_sentence(&sentence) {
-                                    position.long.abs(),
+                        info!("Complete sentence: {}", sentence);
-                                    if position.long >= 0.0 { "E" } else { "W" }
+
-                                );
+                        if sentence.starts_with("$GPGGA") {
-                                info!("  Altitude: {:.1} meters", position.alt);
+                            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) => {
+                        } else if sentence.starts_with("$GPGLL") {
-                                info!("Error parsing GPS data:");
+                            if let Some((lat, long)) = parse_gll(&sentence) {
-                                match e {
+                                info!("Successfully parsed GPS data from GLL:");
-                                    GpsError::InvalidMessageType => {
+                                info!("{}", format_coordinates(lat, long));
-                                        info!("  Invalid message type, expected GGA")
+                                last_position = Some((lat, long));
-                                    }
+                            }
-                                    GpsError::InvalidLength => {
+                        } else if sentence.starts_with("$GPRMC") {
-                                        info!("  Invalid GGA sentence length")
+                            // Parse RMC for speed and course
-                                    }
+                            let parts: Vec<&str> = sentence.split(',').collect();
-                                    GpsError::ParseError(msg) => info!("  Parse error: {}", msg),
+                            if parts.len() >= 13 && parts[2] == "A" {
-                                    GpsError::Other(msg) => info!("  Other error: {}", msg),
+                                if let (Ok(speed), Ok(course)) =
+                                    (parts[7].parse::<f32>(), parts[8].parse::<f32>())
+                                {
+                                    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<dyn std::error::Error>> {
                     // 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<dyn std::error::Error>> {
             }
         }
 
-        // 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
     }
 }