Switch to a library and move main to an example
This commit is contained in:
Robert Sammelson
parent
559fbbaa12
commit
2a43bf94bd
9 changed files with 21 additions and 389 deletions
261
src/device/elm327.rs
Normal file
261
src/device/elm327.rs
Normal file
|
|
@ -0,0 +1,261 @@
|
|||
use log::{debug, info, trace};
|
||||
use std::{
|
||||
collections::VecDeque,
|
||||
io::{Read, Write},
|
||||
thread, time,
|
||||
};
|
||||
|
||||
use super::{Error, Obd2BaseDevice, Obd2Reader, Result};
|
||||
|
||||
pub struct Elm327 {
|
||||
device: ftdi::Device,
|
||||
buffer: VecDeque<u8>,
|
||||
baud_rate: u32,
|
||||
}
|
||||
|
||||
impl Default for Elm327 {
|
||||
fn default() -> Self {
|
||||
Elm327::new().unwrap()
|
||||
}
|
||||
}
|
||||
|
||||
impl Obd2BaseDevice for Elm327 {
|
||||
fn reset(&mut self) -> Result<()> {
|
||||
self.flush_buffers()?;
|
||||
self.reset_ic()?;
|
||||
thread::sleep(time::Duration::from_millis(500));
|
||||
self.reset_protocol()?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn flush(&mut self) -> Result<()> {
|
||||
thread::sleep(time::Duration::from_millis(500));
|
||||
self.read_into_queue()?;
|
||||
self.buffer.clear();
|
||||
thread::sleep(time::Duration::from_millis(500));
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn send_serial_cmd(&mut self, data: &str) -> Result<()> {
|
||||
self.device.write_all(data.as_bytes())?;
|
||||
self.device.write_all(b"\r\n")?;
|
||||
let line = self.get_line()?;
|
||||
if line.as_ref().is_some_and(|v| v == data.as_bytes()) {
|
||||
Ok(())
|
||||
} else {
|
||||
Err(Error::Communication(format!(
|
||||
"send_serial_cmd: got {:?} instead of echoed command ({})",
|
||||
line, data
|
||||
)))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Obd2Reader for Elm327 {
|
||||
fn get_line(&mut self) -> Result<Option<Vec<u8>>> {
|
||||
self.get_until(b'\n', false)
|
||||
}
|
||||
|
||||
fn get_until_prompt(&mut self) -> Result<Option<Vec<u8>>> {
|
||||
self.get_until(b'>', true)
|
||||
}
|
||||
}
|
||||
|
||||
impl Elm327 {
|
||||
fn new() -> Result<Self> {
|
||||
let mut ftdi_device = ftdi::find_by_vid_pid(0x0403, 0x6001)
|
||||
.interface(ftdi::Interface::A)
|
||||
.open()?;
|
||||
|
||||
ftdi_device.set_baud_rate(38400)?;
|
||||
ftdi_device.configure(ftdi::Bits::Eight, ftdi::StopBits::One, ftdi::Parity::None)?;
|
||||
// device.set_latency_timer(2).unwrap();
|
||||
|
||||
ftdi_device.usb_reset()?;
|
||||
|
||||
let mut device = Elm327 {
|
||||
device: ftdi_device,
|
||||
buffer: VecDeque::new(),
|
||||
baud_rate: 38400,
|
||||
};
|
||||
|
||||
device.connect(false)?;
|
||||
device.flush()?;
|
||||
|
||||
Ok(device)
|
||||
}
|
||||
|
||||
fn connect(&mut self, check_baud_rate: bool) -> Result<()> {
|
||||
self.flush_buffers()?;
|
||||
thread::sleep(time::Duration::from_millis(500));
|
||||
self.send_serial_str(" ")?;
|
||||
thread::sleep(time::Duration::from_millis(500));
|
||||
|
||||
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")?;
|
||||
debug!(
|
||||
"reset_ic: got response {:?}",
|
||||
self.get_until_prompt()?
|
||||
.as_ref()
|
||||
.map(|l| std::str::from_utf8(l.as_slice()))
|
||||
);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn reset_protocol(&mut self) -> Result<()> {
|
||||
info!("Performing protocol reset");
|
||||
debug!("reset_protocol: got response {:?}", self.cmd("ATSP0")?);
|
||||
debug!("reset_protocol: got OBD response {:?}", self.cmd("0100")?);
|
||||
self.flush_buffers()?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
let mut buf = Vec::new();
|
||||
let start = time::Instant::now();
|
||||
while start.elapsed() < TIMEOUT {
|
||||
let Some(b) = self.get_byte()? else { continue };
|
||||
let b = match b {
|
||||
b'\r' => Some(b'\n'),
|
||||
b'\n' => None, // no push here
|
||||
_ => Some(b),
|
||||
};
|
||||
if let Some(b) = b {
|
||||
buf.push(b);
|
||||
if b == end_byte {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
trace!(
|
||||
"get_until: got {:?} ({:?})",
|
||||
buf,
|
||||
std::str::from_utf8(buf.as_slice())
|
||||
);
|
||||
|
||||
match buf.pop() {
|
||||
Some(b) if b == end_byte => {
|
||||
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),
|
||||
}
|
||||
}
|
||||
|
||||
fn get_byte(&mut self) -> Result<Option<u8>> {
|
||||
self.read_into_queue()?;
|
||||
loop {
|
||||
let b = self.buffer.pop_front();
|
||||
if b != Some(b'\0') {
|
||||
return Ok(b);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn flush_buffers(&mut self) -> Result<()> {
|
||||
self.device.usb_purge_buffers()?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn send_serial_str(&mut self, data: &str) -> Result<()> {
|
||||
self.device.write_all(data.as_bytes())?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn read_into_queue(&mut self) -> Result<()> {
|
||||
let mut buf = [0u8; 16];
|
||||
loop {
|
||||
let len = self.device.read(&mut buf)?;
|
||||
if len > 0 {
|
||||
self.buffer.extend(&buf[0..len]);
|
||||
trace!(
|
||||
"read_into_queue: values {:?}",
|
||||
std::str::from_utf8(&buf[0..len])
|
||||
);
|
||||
} else {
|
||||
trace!("read_into_queue: no values left to read");
|
||||
break;
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
42
src/device/mod.rs
Normal file
42
src/device/mod.rs
Normal file
|
|
@ -0,0 +1,42 @@
|
|||
mod elm327;
|
||||
pub use elm327::Elm327;
|
||||
|
||||
type Result<T> = std::result::Result<T, Error>;
|
||||
|
||||
pub trait Obd2BaseDevice: Obd2Reader {
|
||||
fn reset(&mut self) -> Result<()>;
|
||||
fn flush(&mut self) -> Result<()>;
|
||||
fn send_serial_cmd(&mut self, data: &str) -> Result<()>;
|
||||
fn cmd(&mut self, cmd: &str) -> Result<Option<String>> {
|
||||
self.send_serial_cmd(cmd)?;
|
||||
self.get_until_prompt()
|
||||
.map(|o| o.and_then(|resp| String::from_utf8(resp).ok()))
|
||||
}
|
||||
}
|
||||
|
||||
pub trait Obd2Reader {
|
||||
fn get_line(&mut self) -> Result<Option<Vec<u8>>>;
|
||||
fn get_until_prompt(&mut self) -> Result<Option<Vec<u8>>>;
|
||||
}
|
||||
|
||||
#[derive(thiserror::Error, Debug)]
|
||||
pub enum Error {
|
||||
#[error("FTDI error: `{0:?}`")]
|
||||
Ftdi(ftdi::Error),
|
||||
#[error("IO error: `{0:?}`")]
|
||||
IO(std::io::Error),
|
||||
#[error("Communication error: `{0}`")]
|
||||
Communication(String),
|
||||
}
|
||||
|
||||
impl From<ftdi::Error> for Error {
|
||||
fn from(e: ftdi::Error) -> Self {
|
||||
Error::Ftdi(e)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<std::io::Error> for Error {
|
||||
fn from(e: std::io::Error) -> Self {
|
||||
Error::IO(e)
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue