Created some very verbose, inefficient and messy code that sends data down an I2C bus for a SSD1306 display to pickup.

src/main.rs

@@ -1,15 +1,26 @@
 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
+    delay::BLOCK,
+    units::Hertz,
+    uart,
+    i2c::{I2cConfig, I2cDriver} 
 };
 use esp_idf_hal;
 use nmea_parser;
 use nmea_parser::gnss::FaaMode;
 
+use embedded_graphics::{
+    prelude::*,
+    image::{Image, ImageRaw},
+    mono_font::{ascii::FONT_7X13, MonoTextStyle},
+    pixelcolor::BinaryColor,
+    text::{Text, Alignment},
+};
+
+use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
+
 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
@@ -20,26 +31,53 @@ fn main() -> Result<(), EspError> {
     // Get Peripherals and sysloop ready
     let peripherals = Peripherals::take().unwrap();
     let pins = peripherals.pins;
+
+    // Define program's pins + addresses
+    let scl = pins.gpio22;
+    let sda = pins.gpio21;
+    let gps_rx  = pins.gpio16;
+    let _lcd_address: u8 = 0x3C;
+
     
     // 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,
+        gps_rx,
         None::<AnyInputPin>,
         None::<AnyOutputPin>,
         &config
     )?;
 
+    // Setup I2C driver
+    let config = I2cConfig::new().baudrate(Hertz(921600));
+    let i2c = I2cDriver::new(
+        peripherals.i2c0, 
+        sda, 
+        scl, 
+        &config
+    )?;
+
+    let interface = I2CDisplayInterface::new(i2c);
+    let mut display = Ssd1306::new(interface, DisplaySize128x64, DisplayRotation::Rotate0)
+        .into_buffered_graphics_mode();
+    display.init().unwrap();
+
+    let raw: ImageRaw<BinaryColor> = ImageRaw::new(include_bytes!("./rust.raw"), 64);
+    let im: Image<'_, _> = Image::new(&raw, Point::new(32, 0));
+
+    im.draw(&mut display).unwrap();
+
+    
     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 {
@@ -47,17 +85,17 @@ fn main() -> Result<(), EspError> {
             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) => {
@@ -85,6 +123,20 @@ fn main() -> Result<(), EspError> {
                 _ => {}
             }
         }
+
+        let style = MonoTextStyle::new(&FONT_7X13, BinaryColor::On);
+
+        display.clear(BinaryColor::Off).unwrap();
+
+        Text::with_alignment(
+            &latest_data.to_string().as_str(),
+            Point::new(64, 10),
+            style,
+            Alignment::Center,
+        )
+        .draw(&mut display).unwrap();
+
+        display.flush().unwrap();
     }
 }
 
@@ -95,3 +147,14 @@ struct GpsData {
     speed: Option<f64>,
     direction: Option<f64>
 }
+
+impl GpsData {
+    pub fn to_string(&self) -> String {
+        format!("Latitude: {:.4?} \nLongitude: {:.4?} \nSpeed: {:.1?}mph \nDirection: {:.4?}deg", 
+            self.latitude.unwrap_or_default(),
+            self.longitude.unwrap_or_default(),
+            self.speed.unwrap_or_default(),
+            self.direction.unwrap_or_default()
+        )
+    }
+}