Add baud rate search function

src/main.rs

@@ -1,3 +1,5 @@
+#![forbid(unsafe_code)]
+
 mod obd2;
 use obd2::Obd2Device;
 

src/obd2/device/elm327.rs

@@ -10,6 +10,7 @@ use super::{Error, Obd2BaseDevice, Obd2Reader, Result};
 pub struct Elm327 {
     device: ftdi::Device,
     buffer: VecDeque<u8>,
+    baud_rate: u32,
 }
 
 impl Default for Elm327 {
@@ -38,6 +39,7 @@ impl Obd2BaseDevice for Elm327 {
     fn send_serial_cmd(&mut self, data: &str) -> Result<()> {
         self.device.write_all(data.as_bytes())?;
         self.device.write_all(b"\r\n")?;
+        // thread::sleep(time::Duration::from_millis(200));
         let line = self.get_line()?;
         if line.as_ref().is_some_and(|v| v == data.as_bytes()) {
             Ok(())
@@ -52,29 +54,11 @@ impl Obd2BaseDevice for Elm327 {
 
 impl Obd2Reader for Elm327 {
     fn get_line(&mut self) -> Result<Option<Vec<u8>>> {
-        let result = self.get_until(b'\n')?;
+        self.get_until(b'\n', false)
-        let Some(mut line) = result else {
-            return Ok(result);
-        };
-        if line.pop() == Some(b'\n') {
-            Ok(Some(line))
-        } else {
-            Err(Error::Communication("get_line no line ending".to_owned()))
-        }
     }
 
     fn get_until_prompt(&mut self) -> Result<Option<Vec<u8>>> {
-        let result = self.get_until(b'>')?;
+        self.get_until(b'>', true)
-        let Some(mut line) = result else {
-            return Ok(result);
-        };
-        if line.pop() == Some(b'>') {
-            Ok(Some(line))
-        } else {
-            Err(Error::Communication(
-                "get_until_prompt no ending".to_owned(),
-            ))
-        }
     }
 }
 
@@ -93,15 +77,16 @@ impl Elm327 {
         let mut device = Elm327 {
             device: ftdi_device,
             buffer: VecDeque::new(),
+            baud_rate: 38400,
         };
 
-        device.connect()?;
+        device.connect(false)?;
         device.flush()?;
 
         Ok(device)
     }
 
-    fn connect(&mut self) -> Result<()> {
+    fn connect(&mut self, check_baud_rate: bool) -> Result<()> {
         self.flush_buffers()?;
         thread::sleep(time::Duration::from_millis(500));
         self.send_serial_str(" ")?;
@@ -109,12 +94,19 @@ impl Elm327 {
 
         self.reset()?;
 
+        if check_baud_rate {
+            match self.find_baud_rate_divisor()? {
+                Some((rate, div)) => info!("Found baud rate {} (divisor {})", rate, div),
+                None => info!("Could not find better baud rate"),
+            }
+        }
+
         Ok(())
     }
 
     fn reset_ic(&mut self) -> Result<()> {
         info!("Performing IC reset");
-        self.send_serial_cmd("atz")?;
+        self.send_serial_cmd("ATZ")?;
         debug!(
             "reset_ic: got response {:?}",
             self.get_until_prompt()?
@@ -126,13 +118,62 @@ impl Elm327 {
 
     fn reset_protocol(&mut self) -> Result<()> {
         info!("Performing protocol reset");
-        debug!("reset_protocol: got response {:?}", self.cmd("atsp0")?);
+        debug!("reset_protocol: got response {:?}", self.cmd("ATSP0")?);
-        debug!("reset_protocol: got response {:?}", self.cmd("0100")?);
+        debug!("reset_protocol: got OBD response {:?}", self.cmd("0100")?);
         self.flush_buffers()?;
         Ok(())
     }
 
-    fn get_until(&mut self, end_byte: u8) -> Result<Option<Vec<u8>>> {
+    fn find_baud_rate_divisor(&mut self) -> Result<Option<(u8, u32)>> {
+        for div in 90..104u8 {
+            let new_baud = 4000000 / u32::from(div);
+
+            debug!("Trying baud rate {} (divisor {})", new_baud, div);
+            self.send_serial_cmd(&format!("ATBRD{:02X}", div))?;
+
+            if self.get_line()? == Some(b"OK".to_vec()) {
+                self.device.set_baud_rate(new_baud)?;
+
+                // validate new baud rate
+                let validation_response = self.get_line()?;
+                if validation_response == Some(b"ELM327 v1.5".to_vec()) {
+                    // reply that it is okay
+                    self.send_serial_str("\r")
+                        .expect("Device left in unknown state");
+                    if self.get_line().expect("Device left in unknown state")
+                        == Some(b"OK".to_vec())
+                    {
+                        self.baud_rate = new_baud;
+                        return Ok(Some((div, new_baud)));
+                    } else {
+                        // our TX is bad
+                        self.device.set_baud_rate(self.baud_rate)?;
+                        debug!("Baud rate bad - device did not receive response");
+                        self.get_until_prompt()?;
+                    }
+                } else {
+                    // reset baud rate and keep looking
+                    self.device.set_baud_rate(self.baud_rate)?;
+                    debug!(
+                        "Baud rate bad - did get correct string (got {:?} - {:?})",
+                        validation_response,
+                        validation_response
+                            .as_ref()
+                            .map(|r| String::from_utf8_lossy(r))
+                    );
+                    self.get_until_prompt()?;
+                }
+            } else {
+                debug!("Baud rate bad - did not ok initially");
+                self.get_until_prompt()?;
+            }
+
+            thread::sleep(time::Duration::from_millis(200));
+        }
+        Ok(None)
+    }
+
+    fn get_until(&mut self, end_byte: u8, allow_empty: bool) -> Result<Option<Vec<u8>>> {
         const TIMEOUT: time::Duration = time::Duration::from_secs(5);
 
         trace!("get_until: getting until {}", end_byte);
@@ -142,20 +183,17 @@ impl Elm327 {
         while start.elapsed() < TIMEOUT {
             let Some(b) = self.get_byte()? else { continue };
             let b = match b {
-                b'\r' => {
+                b'\r' => Some(b'\n'),
-                    buf.push(b'\n');
+                b'\n' => None, // no push here
-                    b'\n'
+                _ => Some(b),
-                }
-                b'\n' => b, // no push here
-                _ => {
-                    buf.push(b);
-                    b
-                }
             };
+            if let Some(b) = b {
+                buf.push(b);
                 if b == end_byte {
                     break;
                 }
             }
+        }
 
         trace!(
             "get_until: got {:?} ({:?})",
@@ -163,13 +201,21 @@ impl Elm327 {
             std::str::from_utf8(buf.as_slice())
         );
 
-        match buf.last() {
+        match buf.pop() {
-            Some(b) if b == &end_byte => Ok(Some(buf)), // we got it
+            Some(b) if b == end_byte => {
-            Some(_) => {
+                if allow_empty || !buf.is_empty() {
+                    Ok(Some(buf))
+                } else {
+                    // empty line, try again
+                    self.get_until(end_byte, allow_empty)
+                }
+            } // we got it
+            Some(f) => {
                 // incomplete line read
                 for b in buf.iter().rev() {
                     self.buffer.push_front(*b);
                 }
+                self.buffer.push_front(f);
                 Ok(None)
             }
             None => Ok(None),