tune via CAN #3361
This commit is contained in:
rusefillc
parent
e429f026f5
commit
99bd024f8e
|
|
@ -0,0 +1,360 @@
|
|||
package com.rusefi.io.can;
|
||||
|
||||
import com.devexperts.logging.Logging;
|
||||
import com.opensr5.io.DataListener;
|
||||
import com.romraider.util.HexUtil;
|
||||
import com.rusefi.io.IoStream;
|
||||
import com.rusefi.io.serial.BaudRateHolder;
|
||||
import com.rusefi.io.serial.SerialIoStreamJSerialComm;
|
||||
import com.rusefi.io.tcp.BinaryProtocolProxy;
|
||||
import com.rusefi.io.tcp.ServerSocketReference;
|
||||
import com.rusefi.io.tcp.TcpConnector;
|
||||
|
||||
import java.io.Closeable;
|
||||
import java.io.IOException;
|
||||
import java.lang.Thread;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
import java.util.concurrent.atomic.AtomicInteger;
|
||||
import java.util.regex.Matcher;
|
||||
import java.util.regex.Pattern;
|
||||
|
||||
public class Elm327Connector implements Closeable, DataListener {
|
||||
private final static Logging log = Logging.getLogging(Elm327Connector.class);
|
||||
private static final byte[] HEX_ARRAY = "0123456789ABCDEF".getBytes();
|
||||
|
||||
private final static int ELM327_DEFAULT_BAUDRATE = 38400;
|
||||
private final static int BIG_TIMEOUT = 2000;
|
||||
private final static int TIMEOUT = 70;
|
||||
|
||||
// these should match the defines in the firmware
|
||||
private final static int CAN_SERIAL_TX_ID = 0x100;
|
||||
private final static int CAN_SERIAL_RX_ID = 0x102;
|
||||
|
||||
private final static int ISO_TP_FRAME_SINGLE = 0;
|
||||
private final static int ISO_TP_FRAME_FIRST = 1;
|
||||
private final static int ISO_TP_FRAME_CONSECUTIVE = 2;
|
||||
private final static int ISO_TP_FRAME_FLOW_CONTROL = 3;
|
||||
|
||||
private IoStream stream = null;
|
||||
private String partialLine = new String();
|
||||
private List<String> completeLines = new ArrayList<String>();
|
||||
private boolean waitForEcho = true;
|
||||
private boolean isCommandMode = false;
|
||||
|
||||
private ServerSocketReference serverHolder;
|
||||
private Elm327IoStream elmStream;
|
||||
|
||||
// CAN multiframe decoder state
|
||||
static class CanDecoder {
|
||||
public int waitingForNumBytes = 0;
|
||||
public int waitingForFrameIndex = 0;
|
||||
|
||||
public byte [] decodePacket(byte [] data) throws Exception {
|
||||
int frameType = (data[0] >> 4) & 0xf;
|
||||
int numBytesAvailable, frameIdx;
|
||||
int dataOffset = 0;
|
||||
switch (frameType) {
|
||||
case ISO_TP_FRAME_SINGLE:
|
||||
numBytesAvailable = data[0] & 0xf;
|
||||
dataOffset = 1;
|
||||
this.waitingForNumBytes = 0;
|
||||
break;
|
||||
case ISO_TP_FRAME_FIRST:
|
||||
this.waitingForNumBytes = ((data[0] & 0xf) << 8) | data[1];
|
||||
this.waitingForFrameIndex = 1;
|
||||
numBytesAvailable = Math.min(this.waitingForNumBytes, 6);
|
||||
dataOffset = 2;
|
||||
break;
|
||||
case ISO_TP_FRAME_CONSECUTIVE:
|
||||
frameIdx = data[0] & 0xf;
|
||||
if (this.waitingForNumBytes < 0 || this.waitingForFrameIndex != frameIdx) {
|
||||
throw new Exception("ISO_TP_FRAME_CONSECUTIVE: That's an abnormal situation, and we probably should react?");
|
||||
}
|
||||
this.waitingForFrameIndex = (this.waitingForFrameIndex + 1) & 0xf;
|
||||
numBytesAvailable = Math.min(this.waitingForNumBytes, 7);
|
||||
dataOffset = 1;
|
||||
break;
|
||||
case ISO_TP_FRAME_FLOW_CONTROL:
|
||||
throw new Exception("ISO_TP_FRAME_FLOW_CONTROL: should we just ignore the FC frame?");
|
||||
default:
|
||||
throw new Exception("Unknown frame type");
|
||||
}
|
||||
|
||||
return Arrays.copyOfRange(data, dataOffset, dataOffset + numBytesAvailable);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public static boolean checkConnection(String serialPort) {
|
||||
Elm327Connector con = new Elm327Connector();
|
||||
boolean found = con.initConnection(serialPort);
|
||||
con.close();
|
||||
return found;
|
||||
}
|
||||
|
||||
public void start(String serialPort) {
|
||||
log.info("* Elm327.start()");
|
||||
|
||||
if (initConnection(serialPort)) {
|
||||
// reset to defaults
|
||||
sendCommand("ATD", "OK");
|
||||
|
||||
// Echo off
|
||||
sendCommand("ATE0", "OK");
|
||||
waitForEcho = false;
|
||||
|
||||
// protocol #6 - ISO 15765-4 CAN (11 bit ID, 500 kbaud)
|
||||
sendCommand("ATSP6", "OK");
|
||||
|
||||
// set rx ID
|
||||
sendCommand("ATCF " + Integer.toHexString(CAN_SERIAL_RX_ID), "OK");
|
||||
|
||||
// rx ID mask = "all bits set"
|
||||
sendCommand("ATCM FFF", "OK");
|
||||
|
||||
// set tx ID
|
||||
sendCommand("ATSH " + Integer.toHexString(CAN_SERIAL_TX_ID), "OK");
|
||||
|
||||
// set FC tx ID (should match our tx ID)
|
||||
sendCommand("ATFCSH " + Integer.toHexString(CAN_SERIAL_TX_ID), "OK");
|
||||
// set FC data
|
||||
sendCommand("ATFCSD 30 00 00", "OK");
|
||||
// use custom FC ID & data
|
||||
sendCommand("ATFCSM 1", "OK");
|
||||
|
||||
// disable data auto-formatting (less bytes in a stream)
|
||||
sendCommand("ATCAF 0", "OK");
|
||||
|
||||
// allow "long" 8-byte messages
|
||||
sendCommand("ATAL", "OK");
|
||||
|
||||
// disable spaces ("00 11 22" => "001122")
|
||||
sendCommand("ATS 0", "OK");
|
||||
|
||||
// disable silent monitoring (ELM will send ACK)
|
||||
sendCommand("ATCSM 0", "OK");
|
||||
|
||||
// read the ignition voltage
|
||||
String voltage = sendCommand("ATRV", "([0-9\\.]+)V");
|
||||
log.info("* Ignition voltage = " + voltage);
|
||||
}
|
||||
|
||||
startNetworkConnector(TcpConnector.DEFAULT_PORT);
|
||||
//sendBytesToSerial(new byte[] { 0, 1, 83, 32, 96, (byte)239, (byte)195 });
|
||||
//sendBytesToSerial(new byte[] { (byte)'V' });
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
log.info("* Elm327.close()");
|
||||
if (stream != null)
|
||||
stream.close();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDataArrived(byte freshData[]) {
|
||||
// ELM327 uses a text protocol, so we convert the data to a string
|
||||
String freshStr = new String(freshData);
|
||||
while (true) {
|
||||
int newL = freshStr.indexOf('\r');
|
||||
//log.info("* onData: " + newL + " [" + freshStr + "]");
|
||||
|
||||
// split the stream into separate lines
|
||||
if (newL >= 0) {
|
||||
String curLine = this.partialLine;
|
||||
this.partialLine = new String();
|
||||
if (newL > 0)
|
||||
curLine += freshStr.substring(0, newL);
|
||||
if (curLine.length() > 0)
|
||||
processLine(curLine);
|
||||
freshStr = freshStr.substring(newL + 1);
|
||||
continue;
|
||||
}
|
||||
this.partialLine += freshStr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public void sendBytesToSerial(byte [] bytes) throws IOException {
|
||||
log.info("-------sendBytesToSerial "+bytes.length+" bytes:");
|
||||
|
||||
for (int i = 0; i < bytes.length; i++) {
|
||||
log.info("["+i+"] " + ((int)bytes[i] & 0xff));
|
||||
}
|
||||
|
||||
// 1 frame
|
||||
if (bytes.length <= 7) {
|
||||
sendFrame((ISO_TP_FRAME_SINGLE << 4) | bytes.length, bytes, 0, bytes.length);
|
||||
return;
|
||||
}
|
||||
|
||||
// multiple frames
|
||||
// send the first header frame
|
||||
sendFrame((ISO_TP_FRAME_FIRST << 4) | ((bytes.length >> 8) & 0x0f), bytes.length & 0xff, bytes, 0, 6);
|
||||
// get a flow control frame
|
||||
byte[] fc = receiveData();
|
||||
|
||||
// send the rest of the data
|
||||
int idx = 1, offset = 6;
|
||||
int remaining = bytes.length - 6;
|
||||
while (remaining > 0) {
|
||||
int len = Math.min(remaining, 7);
|
||||
// send the consecutive frames
|
||||
sendFrame((ISO_TP_FRAME_CONSECUTIVE << 4) | ((idx++) & 0x0f), bytes, offset, len);
|
||||
offset += len;
|
||||
remaining -= len;
|
||||
}
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////
|
||||
|
||||
private boolean initConnection(String serialPort) {
|
||||
// todo: this seems like a hack-ish way? Shouldn't be openPort(port, baudrate)?
|
||||
BaudRateHolder.INSTANCE.baudRate = ELM327_DEFAULT_BAUDRATE;
|
||||
|
||||
this.stream = SerialIoStreamJSerialComm.openPort(serialPort);
|
||||
|
||||
this.stream.setInputListener(this);
|
||||
if (sendCommand("ATZ", "ELM327 v[0-9]+\\.[0-9]+", BIG_TIMEOUT) != null) {
|
||||
log.info("ELM DETECTED on " + serialPort + "!");
|
||||
return true;
|
||||
}
|
||||
log.info("ELM NOT FOUND on " + serialPort + "!");
|
||||
return false;
|
||||
}
|
||||
|
||||
private String sendCommand(String command, String responseFilter) {
|
||||
return sendCommand(command, responseFilter, TIMEOUT);
|
||||
}
|
||||
|
||||
private String sendCommand(String command, String responseFilter, int timeout) {
|
||||
log.info("* Elm327.cmd: " + command);
|
||||
isCommandMode = true;
|
||||
this.completeLines.clear();
|
||||
try {
|
||||
this.stream.write((command + "\r").getBytes());
|
||||
waitForResponse(timeout);
|
||||
} catch (IOException ignore) {
|
||||
return null;
|
||||
} catch (InterruptedException ignore) {
|
||||
return null;
|
||||
} finally {
|
||||
isCommandMode = false;
|
||||
}
|
||||
|
||||
if (this.completeLines.size() < 1) {
|
||||
return null;
|
||||
}
|
||||
|
||||
int responseIdx = 0;
|
||||
// the first line can repeat the command (if echo is on)
|
||||
if (this.completeLines.get(0).equals(command)) {
|
||||
// then the response is in the 2nd line
|
||||
responseIdx = 1;
|
||||
}
|
||||
// return the response only if it matches the given regexp
|
||||
Pattern pattern = Pattern.compile(responseFilter);
|
||||
Matcher matcher = pattern.matcher(this.completeLines.get(responseIdx));
|
||||
if (matcher.find()) {
|
||||
// store the echo mode
|
||||
this.waitForEcho = responseIdx != 0;
|
||||
|
||||
return (matcher.groupCount() > 0) ? matcher.group(1) : matcher.group();
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private void sendFrame(int hdr0, byte [] data, int offset, int len) {
|
||||
sendData(new byte[] { (byte)hdr0 }, data, offset, len);
|
||||
}
|
||||
|
||||
private void sendFrame(int hdr0, int hdr1, byte [] data, int offset, int len) {
|
||||
sendData(new byte[] { (byte)hdr0, (byte)hdr1 }, data, offset, len);
|
||||
}
|
||||
|
||||
private void sendData(byte [] hdr, byte [] data, int offset, int len) {
|
||||
//log.info("--------sendData offset="+Integer.toString(offset) + " len=" + Integer.toString(len) + "hdr.len=" + Integer.toString(hdr.length));
|
||||
|
||||
len += hdr.length;
|
||||
byte [] hexData = new byte [len * 2 + 1];
|
||||
for (int i = 0, j = 0; i < len; i++, j += 2) {
|
||||
int v = ((i < hdr.length) ? hdr[i] : data[i - hdr.length + offset]) & 0xFF;
|
||||
hexData[j] = HEX_ARRAY[v >>> 4];
|
||||
hexData[j + 1] = HEX_ARRAY[v & 0x0F];
|
||||
}
|
||||
hexData[len * 2] = '\r';
|
||||
|
||||
//log.info("* Elm327.data: " + (new String(hexData)));
|
||||
|
||||
try {
|
||||
this.stream.write(hexData);
|
||||
} catch (IOException ignore) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
private synchronized byte[] receiveData() {
|
||||
try {
|
||||
waitForResponse(TIMEOUT);
|
||||
//log.info("Elm327Connector.receiveData(): size=" + this.completeLines.size());
|
||||
return null;
|
||||
} catch (InterruptedException ignore) {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private synchronized void waitForResponse(int timeout) throws InterruptedException {
|
||||
// multiple lines can be sent, we need to wait for them all
|
||||
while (true) {
|
||||
int numLines = this.completeLines.size();
|
||||
wait(timeout);
|
||||
// if nothing changed
|
||||
if (this.completeLines.size() == numLines)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private synchronized void processLine(String line) {
|
||||
log.info("Elm327Connector.processLine(): {" + line + "}");
|
||||
|
||||
// remove the 'cursor'
|
||||
if (line.charAt(0) == '>')
|
||||
line = line.substring(1);
|
||||
|
||||
if (isCommandMode) {
|
||||
// store the output as a response to the command (for verification)
|
||||
this.completeLines.add(line);
|
||||
notifyAll();
|
||||
} else {
|
||||
// just send it back to the proxy
|
||||
sendDataBack(line);
|
||||
}
|
||||
}
|
||||
|
||||
private void sendDataBack(String line) {
|
||||
byte [] canPacket = HexUtil.asBytes(line);
|
||||
try {
|
||||
elmStream.processCanPacket(canPacket);
|
||||
} catch (Exception ignore) {
|
||||
// todo: ?
|
||||
}
|
||||
}
|
||||
|
||||
private boolean startNetworkConnector(int controllerPort) {
|
||||
try {
|
||||
elmStream = new Elm327IoStream(this, "elm327Stream");
|
||||
serverHolder = BinaryProtocolProxy.createProxy(elmStream, controllerPort, new BinaryProtocolProxy.ClientApplicationActivityListener() {
|
||||
@Override
|
||||
public void onActivity() {
|
||||
}
|
||||
});
|
||||
} catch (IOException ignore) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
package com.rusefi.io.can;
|
||||
|
||||
import com.opensr5.io.DataListener;
|
||||
import com.rusefi.binaryprotocol.IncomingDataBuffer;
|
||||
import com.rusefi.binaryprotocol.IoHelper;
|
||||
import com.rusefi.io.ByteReader;
|
||||
import com.rusefi.io.serial.AbstractIoStream;
|
||||
import com.rusefi.shared.FileUtil;
|
||||
import org.jetbrains.annotations.NotNull;
|
||||
|
||||
import java.io.*;
|
||||
import java.nio.ByteBuffer;
|
||||
import java.util.Arrays;
|
||||
|
||||
public class Elm327IoStream extends AbstractIoStream {
|
||||
private final String loggingPrefix;
|
||||
private final Elm327Connector con;
|
||||
private final DisconnectListener disconnectListener;
|
||||
@NotNull
|
||||
private final IncomingDataBuffer dataBuffer;
|
||||
private DataListener dataListener;
|
||||
|
||||
// the buffer size is limited by CAN-TP protocol
|
||||
private final static int OUT_BUFFER_SIZE = 4095;
|
||||
private ByteBuffer inBuf;
|
||||
private ByteBuffer outBuf;
|
||||
|
||||
// this should match the TS_CAN_DEVICE_SHORT_PACKETS_IN_ONE_FRAME in the firmware
|
||||
private final static boolean sendShortPacketsInOneFrame = true;
|
||||
private final static boolean receiveShortPacketsInOneFrame = false;
|
||||
|
||||
private Elm327Connector.CanDecoder canDecoder = new Elm327Connector.CanDecoder();
|
||||
|
||||
|
||||
public Elm327IoStream(Elm327Connector con, String loggingPrefix) throws IOException {
|
||||
this(con, loggingPrefix, DisconnectListener.VOID);
|
||||
}
|
||||
|
||||
public Elm327IoStream(Elm327Connector con, String loggingPrefix, DisconnectListener disconnectListener) throws IOException {
|
||||
this.con = con;
|
||||
this.loggingPrefix = loggingPrefix;
|
||||
this.disconnectListener = disconnectListener;
|
||||
this.dataBuffer = IncomingDataBuffer.createDataBuffer(loggingPrefix, this);
|
||||
|
||||
inBuf = ByteBuffer.allocate(OUT_BUFFER_SIZE);
|
||||
outBuf = ByteBuffer.allocate(OUT_BUFFER_SIZE);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
// we need to guarantee only one onDisconnect invocation for retry logic to be healthy
|
||||
synchronized (this) {
|
||||
if (!isClosed()) {
|
||||
super.close();
|
||||
disconnectListener.onDisconnect("on close");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getLoggingPrefix() {
|
||||
return loggingPrefix;
|
||||
}
|
||||
|
||||
@Override
|
||||
public IncomingDataBuffer getDataBuffer() {
|
||||
return dataBuffer;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void write(byte[] bytes) throws IOException {
|
||||
super.write(bytes);
|
||||
|
||||
int offset = 0;
|
||||
int numBytes = bytes.length;
|
||||
// split the data if it doesn't fit in our out-buffer
|
||||
while (numBytes > outBuf.remaining()) {
|
||||
int remaining = outBuf.remaining();
|
||||
//log.info("* remaining= "+remaining + " numBytes="+numBytes+" offset="+offset);
|
||||
outBuf.put(bytes, offset, remaining);
|
||||
numBytes -= remaining;
|
||||
offset += remaining;
|
||||
flush();
|
||||
}
|
||||
//log.info("* rest numBytes="+numBytes+" offset="+offset);
|
||||
outBuf.put(bytes, offset, numBytes);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void flush() throws IOException {
|
||||
super.flush();
|
||||
byte [] bytes;
|
||||
// for smaller packets, send them in one 'simple' frame by stripping the header+footer off
|
||||
// (2 = 16-bit length, 4 = 32-bit crc)
|
||||
if (sendShortPacketsInOneFrame && outBuf.position() >= 2 + 1 + 4 && outBuf.position() <= 2 + 7 + 4) {
|
||||
bytes = Arrays.copyOfRange(outBuf.array(), 2, outBuf.position() - 4);
|
||||
} else {
|
||||
bytes = Arrays.copyOf(outBuf.array(), outBuf.position());
|
||||
}
|
||||
con.sendBytesToSerial(bytes);
|
||||
outBuf.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setInputListener(final DataListener listener) {
|
||||
dataListener = listener;
|
||||
}
|
||||
|
||||
public void sendDataToClient(byte [] data) {
|
||||
// reconstruct a TS packet from the raw data
|
||||
byte [] packet;
|
||||
if (receiveShortPacketsInOneFrame && data.length >= 1 && data.length <= 7) {
|
||||
// short packets are sent without the header+footer, we need to reconstruct them
|
||||
packet = new byte [2 + data.length + 4];
|
||||
IoHelper.putShort(packet, 0, data.length);
|
||||
|
||||
for (int i = 0; i < data.length; i++) {
|
||||
packet[i + 2] = data[i];
|
||||
}
|
||||
int crc = IoHelper.getCrc32(data);
|
||||
IoHelper.putInt(packet, 2 + data.length, crc);
|
||||
} else {
|
||||
// long packets are sent as-is
|
||||
packet = data;
|
||||
}
|
||||
|
||||
dataListener.onDataArrived(packet);
|
||||
}
|
||||
|
||||
public void processCanPacket(byte [] data) throws Exception {
|
||||
byte [] rawData = canDecoder.decodePacket(data);
|
||||
sendDataToClient(rawData);
|
||||
}
|
||||
|
||||
public interface DisconnectListener {
|
||||
DisconnectListener VOID = (String message) -> {
|
||||
|
||||
};
|
||||
void onDisconnect(String message);
|
||||
}
|
||||
}
|
||||
|
|
@ -38,6 +38,8 @@ public class SerialIoStreamJSerialComm extends AbstractIoStream {
|
|||
|
||||
@Override
|
||||
public void setInputListener(DataListener listener) {
|
||||
// datalistener can be redefined
|
||||
sp.removeDataListener();
|
||||
sp.addDataListener(new SerialPortDataListener() {
|
||||
private boolean isFirstEvent = true;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,21 @@
|
|||
package com.rusefi.tools;
|
||||
|
||||
import com.devexperts.logging.Logging;
|
||||
import com.rusefi.autodetect.PortDetector;
|
||||
import com.rusefi.io.can.Elm327Connector;
|
||||
|
||||
public class Elm327ConnectorStartup {
|
||||
private final static Logging log = Logging.getLogging(Elm327ConnectorStartup.class);
|
||||
|
||||
public static void start() {
|
||||
//String autoDetectedPort = PortDetector.autoDetectSerial(null, PortDetector.DetectorMode.DETECT_ELM327);
|
||||
String autoDetectedPort = "COM73";
|
||||
if (autoDetectedPort == null) {
|
||||
System.err.println(ConsoleTools.RUS_EFI_NOT_DETECTED);
|
||||
return;
|
||||
}
|
||||
|
||||
(new Elm327Connector()).start(autoDetectedPort);
|
||||
log.info("Running Elm327 connector for " + autoDetectedPort);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue