From 1f8fe3020ffc50691db385c0dd86857fbd2d6962 Mon Sep 17 00:00:00 2001
From: Luke Else <mail@luke-else.co.uk>
Date: Fri, 14 Jul 2023 12:23:13 +0100
Subject: [PATCH] Finished code to have data get read from a serial GPS module

---
 Cargo.toml  |  1 +
 src/main.rs | 93 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 2 files changed, 90 insertions(+), 4 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 871951b..1ddf85f 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -33,6 +33,7 @@ esp-idf-sys = { version = "0.33", default-features = false }
 esp-idf-hal = { version = "0.41", optional = true, default-features = false }
 esp-idf-svc = { version = "0.46", optional = true, default-features = false }
 embedded-svc = { version = "0.25", optional = true, default-features = false }
+nmea-parser = "0.10.0"
 
 [build-dependencies]
 embuild = "0.31.2"
diff --git a/src/main.rs b/src/main.rs
index c0b518d..2e61a6e 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,12 +1,97 @@
-use esp_idf_sys as _; // If using the `binstart` feature of `esp-idf-sys`, always keep this module imported
-use log::*;
+use esp_idf_sys::{self as _, EspError}; // If using the `binstart` feature of `esp-idf-sys`, always keep this module imported
+use esp_idf_hal::{
+    prelude::Peripherals, 
+    uart, 
+    units::Hertz,
+    gpio::{AnyInputPin, AnyOutputPin},
+    delay::BLOCK
+};
+use esp_idf_hal;
+use nmea_parser;
+use nmea_parser::gnss::FaaMode;
 
-fn main() {
+fn main() -> Result<(), EspError> {
     // It is necessary to call this function once. Otherwise some patches to the runtime
     // implemented by esp-idf-sys might not link properly. See https://github.com/esp-rs/esp-idf-template/issues/71
     esp_idf_sys::link_patches();
     // Bind the log crate to the ESP Logging facilities
     esp_idf_svc::log::EspLogger::initialize_default();
 
-    info!("Hello, world!");
+    // Get Peripherals and sysloop ready
+    let peripherals = Peripherals::take().unwrap();
+    let pins = peripherals.pins;
+    
+    // Setup UART to read serial data from GPS
+    let config = uart::config::Config::default().baudrate(Hertz(9_600));
+    let uart: uart::UartRxDriver = uart::UartRxDriver::new(
+        peripherals.uart1,
+        pins.gpio16,
+        None::<AnyInputPin>,
+        None::<AnyOutputPin>,
+        &config
+    )?;
+
+    let mut nmea_parser = nmea_parser::NmeaParser::new();
+    let mut latest_data = GpsData{latitude: None, longitude: None, speed: None, direction: None};
+
+    loop {
+        // Read buffer from UART
+        let mut buf: Vec<u8> = (0..uart.count()? as u8).collect();
+        uart.read(&mut buf[..], BLOCK)?;
+        std::thread::sleep(std::time::Duration::from_millis(10));
+
+        let nmea_raw = String::from_utf8(buf).unwrap_or_default();
+        let nmea_vec: Vec<&str> = nmea_raw.split('$').collect();
+        for nmea_line in nmea_vec {
+            // Don't try and process / parse if the string is empty
+            if nmea_line.is_empty() {
+                continue;
+            }
+
+            // Construct string that is in the correct format for parsing
+            let mut sentence = "$".to_string();
+            sentence.push_str(nmea_line);
+
+            let nmea = match nmea_parser.parse_sentence(sentence.as_str()) {
+                Ok(nmea) => nmea,
+                // Don't continue processin sentence if we know that it isn't supported
+                Err(_) => { continue; }
+            };
+
+            // print decoded gps data to serial
+            match nmea {
+                nmea_parser::ParsedMessage::Gll(gll) => {
+                    println!("Latitude: {:.6?}", gll.latitude.unwrap_or_default());
+                    println!("Longitude: {:.6?}", gll.longitude.unwrap_or_default());
+                    println!("FAA Mode: {:?}m", gll.faa_mode.unwrap_or(nmea_parser::gnss::FaaMode::NotValid));
+                    println!("Data Valid: {:?}", gll.data_valid.unwrap_or(false));
+                    if gll.faa_mode.unwrap_or(FaaMode::NotValid) == FaaMode::Autonomous {
+                        latest_data.latitude = gll.latitude;
+                        latest_data.longitude = gll.longitude;
+                    }
+                    println!("\n\n");
+                },
+                nmea_parser::ParsedMessage::Gns(gns) => {
+                    println!("Altitude: {:.2?}", gns.altitude.unwrap_or_default());
+                    println!("Num Sat: {:?}", gns.satellite_count.unwrap_or_default());
+                    println!("Time: {:?}", gns.timestamp.unwrap_or_default());
+                },
+                nmea_parser::ParsedMessage::Rmc(rms) => {
+                    println!("Speed: {:.2?}mph", rms.sog_knots.unwrap_or_default() * 1.150779);
+                    println!("Current Track: {:.2?}deg", rms.bearing.unwrap_or_default());
+                    latest_data.speed = Some(rms.sog_knots.unwrap_or_default() * 1.150779);
+                    latest_data.direction = rms.bearing;
+                },
+                _ => {}
+            }
+        }
+    }
+}
+
+/// Data structure to store all relevant data collected from the gps
+struct GpsData {
+    latitude: Option<f64>,
+    longitude: Option<f64>,
+    speed: Option<f64>,
+    direction: Option<f64>
 }