copy-paste ublox serial config and reading

Cargo.toml

@@ -19,7 +19,7 @@ esp-idf-hal = "0.37.4"
 esp-idf-sys = { version = "0.31.5", features = ["binstart", "native"] }
 mqtt-protocol = "0.11.2"
 nb = "1.0.0"
-ublox = "0.4.2"
+ublox = { git = "https://github.com/lkolbly/ublox.git", branch = "master" }
 
 [build-dependencies]
 embuild = "0.29"

sdkconfig.defaults

@@ -5,6 +5,9 @@ CONFIG_ESP_MAIN_TASK_STACK_SIZE=64000
 # This allows to use 1 ms granuality for thread sleeps (10 ms by default).
 #CONFIG_FREERTOS_HZ=1000
 
+# Explicitly specify UART0 for console debugging.
+CONFIG_CONSOLE_UART_NUM=0
+
 # Workaround for https://github.com/espressif/esp-idf/issues/7631
 CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=n
 CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL=n

src/gps.rs

@@ -3,21 +3,90 @@ use anyhow;
 use std::{
     sync::mpsc::SyncSender,
     thread,
-    time::Duration,
-    io::Read,
+    time::{Duration, Instant},
+    io::{Read, Write},
 };
 
 use esp_idf_hal::prelude::*;
 use esp_idf_hal::serial;
+use esp_idf_hal::gpio::*;
 
 use ublox::*;
 
 use crate::modem::Msg;
 use crate::serial::SerialIO;
 
+struct Device<UART: serial::Uart> {
+    port: SerialIO<UART>,
+    parser: Parser<Vec<u8>>,
+}
+
+impl<UART: serial::Uart> Device<UART> {
+    pub fn new<T: Sync + Send>(port: serial::Serial<UART, Gpio12<T>, Gpio13<T>>) -> Device<UART> {
+        let parser = Parser::default();
+        let (tx, rx) = port.split();
+        Device { port: SerialIO::new(tx, rx), parser }
+    }
+
+    pub fn write_all(&mut self, data: &[u8]) -> std::io::Result<()> {
+        println!("WRITE: {:?}", data);
+        self.port.write(data).map(|_| ())
+    }
+
+    pub fn update<T: FnMut(PacketRef)>(&mut self, mut cb: T) -> std::io::Result<()> {
+        println!("UPDATING ...     ...     ...");
+        loop {
+            let mut local_buf = [0; 1024];
+            let nbytes = self.read_port(&mut local_buf)?;
+            if nbytes == 0 {
+                break;
+            }
+            // parser.consume adds the buffer to its internal buffer, and
+            // returns an iterator-like object we can use to process the packets
+            let mut it = self.parser.consume(&local_buf[..nbytes]);
+            while let Some(Ok(packet)) = it.next() {
+                println!("READ: {:?}", packet);
+                cb(packet);
+            }
+        }
+        Ok(())
+    }
+
+    pub fn wait_for_ack<T: UbxPacketMeta>(&mut self, timeout: Duration) -> std::io::Result<()> {
+        let mut found_packet = false;
+        println!("LOOKING FOR ACK ...");
+        let start = Instant::now();
+        while !found_packet && start.elapsed() < timeout {
+            self.update(|packet| {
+                if let PacketRef::AckAck(ack) = packet {
+                    if ack.class() == T::CLASS && ack.msg_id() == T::ID {
+                        println!("FOUND PACKET: {} {}", ack.class(), ack.msg_id());
+                        found_packet = true;
+                    }
+                }
+            })?;
+        }
+        Ok(())
+    }
+
+    /// Reads the serial port, converting timeouts into "no data received"
+    fn read_port(&mut self, output: &mut [u8]) -> std::io::Result<usize> {
+        match self.port.read(output) {
+            Ok(b) => Ok(b),
+            Err(e) => {
+                if e.kind() == std::io::ErrorKind::TimedOut {
+                    Ok(0)
+                } else {
+                    Err(e)
+                }
+            }
+        }
+    }
+}
+
 pub fn main<T: Sync + Send>(
-    tx: esp_idf_hal::gpio::Gpio33<T>,
-    rx: esp_idf_hal::gpio::Gpio32<T>,
+    tx: esp_idf_hal::gpio::Gpio12<T>,
+    rx: esp_idf_hal::gpio::Gpio13<T>,
     uart: serial::UART2,
     sender: SyncSender<Msg>,
 ) -> Result<(), anyhow::Error> {
@@ -27,46 +96,94 @@ pub fn main<T: Sync + Send>(
         cts: None,
         rts: None,
     };
+
+    let serial_config = serial::config::Config::default()
+        .baudrate(Hertz(9600))
+        .data_bits(serial::config::DataBits::DataBits8)
+        .parity_none()
+        .flow_control(serial::config::FlowControl::None)
+        .stop_bits(serial::config::StopBits::STOP1);
+
     let serial: serial::Serial<serial::UART2, _, _> = serial::Serial::new(
         uart,
         serial_pins,
-        serial::config::Config::default().baudrate(Hertz(9600)),
+        serial_config,
     )?;
 
-    let (tx, rx) = serial.split();
-    let mut serial_io = SerialIO::new(tx, rx);
-    let mut parser = Parser::default();
+    let mut device = Device::new(serial);
 
-    println!("entering GPS sender loop ...");
-    loop {
-        let mut local_buf = [0; 100];
-        println!("reading 100 bytes from serial ...");
-        let nbytes = serial_io.read(&mut local_buf)?;
-        println!("READ: {}", local_buf.iter().map(|&b| char::from(b)).collect::<String>());
-        if nbytes == 0 {
-            println!("received 0 bytes, exiting ...");
-            break;
-        }
-        let mut it = parser.consume(&local_buf);
-
-        loop {
-            match it.next() {
-                Some(Ok(packet)) => {
-                    let msg = format!("We've received a &PacketRef, we can handle it ... {:?}", packet);
-                    println!("{}", msg);
-                    sender.send(Msg::Location(msg))?;
-                }
-                Some(Err(err)) => {
-                    println!("Received a malformed packet {:?}", err);
-                }
-                None => {
-                    println!("The internal buffer is now empty");
-                    break;
-                }
+    // Configure the device to talk UBX
+    device
+        .write_all(
+            &CfgPrtUartBuilder {
+                portid: UartPortId::Uart1,
+                reserved0: 0,
+                tx_ready: 0,
+                mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One),
+                baud_rate: 9600,
+                in_proto_mask: InProtoMask::all(),
+                out_proto_mask: OutProtoMask::UBLOX,
+                flags: 0,
+                reserved5: 0,
             }
-        }
-        thread::sleep(Duration::from_millis(2000));
+            .into_packet_bytes(),
+        )
+        .unwrap();
+    device.wait_for_ack::<CfgPrtUart>(Duration::from_millis(3000)).unwrap();
+    println!("CfgPrtUart acked!");
+
+    // Enable the NavPosVelTime packet
+    device
+        .write_all(
+            &CfgMsgAllPortsBuilder::set_rate_for::<NavPosVelTime>([0, 1, 0, 0, 0, 0])
+                .into_packet_bytes(),
+        )
+        .unwrap();
+    device.wait_for_ack::<CfgMsgAllPorts>(Duration::from_millis(3000)).unwrap();
+    println!("CfgMsgAllPorts acked!");
+
+    // Send a packet request for the MonVer packet
+    device
+        .write_all(&UbxPacketRequest::request_for::<MonVer>().into_packet_bytes())
+        .unwrap();
+
+    // Start reading data
+    println!("Opened u-blox device, waiting for solutions...");
+    for _ in 0..20 {
+        device
+            .update(|packet| match packet {
+                PacketRef::MonVer(packet) => {
+                    println!(
+                        "SW version: {} HW version: {}",
+                        packet.software_version(),
+                        packet.hardware_version()
+                    );
+                    println!("{:?}", packet);
+                }
+                PacketRef::NavPosVelTime(sol) => {
+                    let has_posvel = sol.fix_type() == GpsFix::Fix3D
+                        || sol.fix_type() == GpsFix::GPSPlusDeadReckoning;
+
+                    if has_posvel {
+                        let pos: Position = (&sol).into();
+                        let vel: Velocity = (&sol).into();
+                        println!(
+                            "Latitude: {:.5} Longitude: {:.5} Altitude: {:.2}m",
+                            pos.lat, pos.lon, pos.alt
+                        );
+                        println!(
+                            "Speed: {:.2} m/s Heading: {:.2} degrees",
+                            vel.speed, vel.heading
+                        );
+                        println!("Sol: {:?}", sol);
+                    }
+                }
+                _ => {
+                    println!("{:?}", packet);
+                }
+            })?
     }
+
     println!("exiting GPS sender loop :)");
     Ok(())
 }

src/main.rs

@@ -31,11 +31,11 @@ fn main() -> anyhow::Result<()> {
     // UART interface for the GSM modem
     let modem_uart = dp.uart1;
 
-    let mut threads: Vec<JoinHandle<anyhow::Result<_>>> = vec![];
+    // let mut threads: Vec<JoinHandle<anyhow::Result<_>>> = vec![];
 
     // Rx/Tx pins for the GPS modem
-    let gps_rx = dp.pins.gpio32;
-    let gps_tx = dp.pins.gpio33;
+    let gps_rx = dp.pins.gpio13;
+    let gps_tx = dp.pins.gpio12;
     // UART interface for the GPS modem
     let gps_uart = dp.uart2;
 
@@ -46,11 +46,11 @@ fn main() -> anyhow::Result<()> {
 
     let _ = gps::main(gps_tx, gps_rx, gps_uart, gps_sender)?;
     // threads.push(thread::spawn(move || gps::main(gps_rx, gps_tx, gps_uart, gps_sender)));
-    thread::sleep(Duration::from_millis(1000));
-    threads.push(thread::spawn(move || accel::main(accel_sender)));
-    thread::sleep(Duration::from_millis(1000));
+    //thread::sleep(Duration::from_millis(1000));
+    //threads.push(thread::spawn(move || accel::main(accel_sender)));
+    //thread::sleep(Duration::from_millis(1000));
 
-    let _ = modem::main(modem_rx, modem_tx, modem_uart, modem_pwrkey, modem_rst, modem_power, receiver)?;
+    //let _ = modem::main(modem_rx, modem_tx, modem_uart, modem_pwrkey, modem_rst, modem_power, receiver)?;
 
     Ok(())
 }

src/serial.rs

@@ -132,6 +132,6 @@ impl<UART: serial::Uart> io::Write for SerialIO<UART> {
 
     fn flush(&mut self) -> io::Result<()> {
         self.tx.flush()
-            .map_err(|err| io::Error::from(io::ErrorKind::Other))
+            .map_err(|_| io::Error::from(io::ErrorKind::Other))
     }
 }