From 545fa95f17f2146b61e2cb223502bd3e3bc7fcb3 Mon Sep 17 00:00:00 2001 From: Vladan Popovic Date: Tue, 12 Jul 2022 19:10:04 +0200 Subject: [PATCH] copy-paste ublox serial config and reading --- Cargo.toml | 2 +- sdkconfig.defaults | 3 + src/gps.rs | 189 ++++++++++++++++++++++++++++++++++++--------- src/main.rs | 14 ++-- src/serial.rs | 2 +- 5 files changed, 165 insertions(+), 45 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 4ff3999..5e56188 100644 --- a/Cargo.toml +++ b/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" diff --git a/sdkconfig.defaults b/sdkconfig.defaults index a0f1158..cd6dab8 100644 --- a/sdkconfig.defaults +++ b/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 diff --git a/src/gps.rs b/src/gps.rs index 03115c6..4d356f4 100644 --- a/src/gps.rs +++ b/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 { + port: SerialIO, + parser: Parser>, +} + +impl Device { + pub fn new(port: serial::Serial, Gpio13>) -> Device { + 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(&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(&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 { + 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( - tx: esp_idf_hal::gpio::Gpio33, - rx: esp_idf_hal::gpio::Gpio32, + tx: esp_idf_hal::gpio::Gpio12, + rx: esp_idf_hal::gpio::Gpio13, uart: serial::UART2, sender: SyncSender, ) -> Result<(), anyhow::Error> { @@ -27,46 +96,94 @@ pub fn main( 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::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::()); - 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::(Duration::from_millis(3000)).unwrap(); + println!("CfgPrtUart acked!"); + + // Enable the NavPosVelTime packet + device + .write_all( + &CfgMsgAllPortsBuilder::set_rate_for::([0, 1, 0, 0, 0, 0]) + .into_packet_bytes(), + ) + .unwrap(); + device.wait_for_ack::(Duration::from_millis(3000)).unwrap(); + println!("CfgMsgAllPorts acked!"); + + // Send a packet request for the MonVer packet + device + .write_all(&UbxPacketRequest::request_for::().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(()) } diff --git a/src/main.rs b/src/main.rs index c5fbf50..3265b58 100644 --- a/src/main.rs +++ b/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>> = vec![]; + // let mut threads: Vec>> = 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(()) } diff --git a/src/serial.rs b/src/serial.rs index 1fbc629..fcc9aeb 100644 --- a/src/serial.rs +++ b/src/serial.rs @@ -132,6 +132,6 @@ impl io::Write for SerialIO { 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)) } }