426 lines
15 KiB
Python
Executable file
426 lines
15 KiB
Python
Executable file
#!/usr/bin/env python3
|
|
"""
|
|
OBD-II / ELM327 Bluetooth Emulator for ESP32 CYD Gauge
|
|
Simulates an ELM327 v1.5 Bluetooth adapter and vehicle ECU.
|
|
"""
|
|
|
|
import sys
|
|
import os
|
|
import time
|
|
import math
|
|
import select
|
|
import socket
|
|
import threading
|
|
import dbus
|
|
import dbus.service
|
|
from dbus.mainloop.glib import DBusGMainLoop
|
|
from gi.repository import GLib
|
|
|
|
# Vehicle Simulation State
|
|
class VehicleState:
|
|
def __init__(self):
|
|
self.rpm = 1800.0 # RPM
|
|
self.target_rpm = 2200.0 # Target RPM for smooth transitions
|
|
self.speed = 45.0 # km/h
|
|
self.coolant_temp = 88.0 # °C
|
|
self.oil_temp = 92.0 # °C
|
|
self.trans_temp = 82.0 # °C (Ford T6 221E1C)
|
|
self.load = 32.0 # % (0-100)
|
|
self.map_psi = 14.7 # psi (Manifold Absolute Pressure)
|
|
self.pcm_volt = 14.2 # Volts (MUST be > 12.5V or gauge will sleep)
|
|
self.auto_simulate = True
|
|
self.lock = threading.Lock()
|
|
|
|
def update_simulation(self):
|
|
"""Simulates driving telemetry with clear transitions through color zones and holds at max scale"""
|
|
t = time.time()
|
|
with self.lock:
|
|
if self.auto_simulate:
|
|
# 24-second cycle:
|
|
# 0 - 8s: Ramp up 800 -> 6000 RPM (Verde -> Amarillo -> Rojo)
|
|
# 8 - 12s: Hold at MAX (6000 RPM, redline/escala máxima) for 4s
|
|
# 12 - 20s: Ramp down 6000 -> 800 RPM (Rojo -> Amarillo -> Verde)
|
|
# 20 - 24s: Hold at IDLE (800 RPM) for 4s
|
|
cycle_period = 24.0
|
|
phase = t % cycle_period
|
|
|
|
if phase < 8.0:
|
|
p = phase / 8.0
|
|
self.rpm = 800.0 + p * (6000.0 - 800.0)
|
|
elif phase < 12.0:
|
|
self.rpm = 6000.0
|
|
elif phase < 20.0:
|
|
p = (phase - 12.0) / 8.0
|
|
self.rpm = 6000.0 - p * (6000.0 - 800.0)
|
|
else:
|
|
self.rpm = 800.0
|
|
|
|
# MAP pressure correlates with RPM
|
|
self.map_psi = 10.0 + 16.0 * ((self.rpm - 800.0) / 5200.0)
|
|
# Engine load correlates with RPM
|
|
self.load = 15.0 + 75.0 * ((self.rpm - 800.0) / 5200.0)
|
|
# Vehicle speed correlates with RPM (up to 175 km/h, turning RED past 130 km/h)
|
|
self.speed = 15.0 + 160.0 * ((self.rpm - 800.0) / 5200.0)
|
|
if self.speed < 0:
|
|
self.speed = 0.0
|
|
# Coolant temp rises with engine load (88°C -> 108°C: Green <98, Yellow 98-105, Red >=106)
|
|
self.coolant_temp = 88.0 + 20.0 * ((self.rpm - 800.0) / 5200.0)
|
|
self.oil_temp = 90.0 + 12.0 * ((self.rpm - 800.0) / 5200.0)
|
|
self.trans_temp = 82.0 + 1.0 * math.cos(t * 0.09)
|
|
self.pcm_volt = 14.2 + 0.15 * math.sin(t * 0.5)
|
|
|
|
state = VehicleState()
|
|
|
|
# BlueZ Profile for Serial Port Profile (SPP)
|
|
class SPPProfile(dbus.service.Object):
|
|
def __init__(self, bus, path):
|
|
super().__init__(bus, path)
|
|
|
|
@dbus.service.method('org.bluez.Profile1', in_signature='', out_signature='')
|
|
def Release(self):
|
|
print("[BlueZ] Profile released")
|
|
|
|
@dbus.service.method('org.bluez.Profile1', in_signature='oha{sv}', out_signature='')
|
|
def NewConnection(self, path, fd, properties):
|
|
raw_fd = fd.take()
|
|
print(f"\n[Bluetooth D-Bus] ¡Nueva conexión aceptada desde {path}! (fd={raw_fd})")
|
|
try:
|
|
client_sock = socket.fromfd(raw_fd, socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
|
|
t = threading.Thread(target=handle_client_stream, args=(client_sock,), daemon=True)
|
|
t.start()
|
|
except Exception as e:
|
|
print(f"[Bluetooth] Error al inicializar socket desde fd: {e}")
|
|
|
|
@dbus.service.method('org.bluez.Profile1', in_signature='o', out_signature='')
|
|
def RequestDisconnection(self, path):
|
|
print(f"[BlueZ] RequestDisconnection from {path}")
|
|
|
|
|
|
class AutoAgent(dbus.service.Object):
|
|
"""BlueZ Agent that automatically accepts pairing and SPP authorization"""
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="", out_signature="")
|
|
def Release(self):
|
|
pass
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="os", out_signature="")
|
|
def AuthorizeService(self, device, uuid):
|
|
print(f"\n[BlueZ Agent] Conexión de servicio autorizada para {device}")
|
|
return
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="s")
|
|
def RequestPinCode(self, device):
|
|
print(f"\n[BlueZ Agent] PIN solicitado para {device} -> 1234")
|
|
return "1234"
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="u")
|
|
def RequestPasskey(self, device):
|
|
return dbus.UInt32(0)
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="ouq", out_signature="")
|
|
def DisplayPasskey(self, device, passkey, entered):
|
|
pass
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="os", out_signature="")
|
|
def DisplayPinCode(self, device, pincode):
|
|
pass
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="ou", out_signature="")
|
|
def RequestConfirmation(self, device, passkey):
|
|
return
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="o", out_signature="")
|
|
def RequestAuthorization(self, device):
|
|
return
|
|
|
|
@dbus.service.method("org.bluez.Agent1", in_signature="", out_signature="")
|
|
def Cancel(self):
|
|
pass
|
|
|
|
|
|
def format_elm_response(cmd: str) -> str:
|
|
"""Computes the ELM327 response string for a given command"""
|
|
cmd = cmd.strip().upper()
|
|
|
|
if not cmd:
|
|
return "OK\r\r>"
|
|
|
|
# AT Commands
|
|
if cmd.startswith("AT"):
|
|
if cmd == "ATZ" or cmd == "ATWS":
|
|
return "ELM327 v1.5\r\r>"
|
|
elif cmd in ["ATE0", "ATE1", "ATL0", "ATL1", "ATH0", "ATH1", "ATSP0", "ATAT2", "ATBRD23", "ATLP", "ATPP0CSV35", "ATPP0CON"]:
|
|
return "OK\r\r>"
|
|
elif cmd == "ATDP" or cmd == "ATDPN":
|
|
return "AUTO, ISO 15765-4 (CAN 11/500)\r\r>"
|
|
elif cmd == "ATRV":
|
|
return f"{state.pcm_volt:.1f}V\r\r>"
|
|
elif cmd == "ATIGN":
|
|
return "ON\r\r>"
|
|
else:
|
|
return "OK\r\r>"
|
|
|
|
# OBD2 PIDs
|
|
with state.lock:
|
|
rpm = state.rpm
|
|
load = state.load
|
|
coolant = state.coolant_temp
|
|
speed = state.speed
|
|
oil = state.oil_temp
|
|
trans = state.trans_temp
|
|
map_p = state.map_psi
|
|
volt = state.pcm_volt
|
|
|
|
if cmd == "0100":
|
|
# PIDs 01-20 supported
|
|
return "41 00 BE 3F B8 10\r\r>"
|
|
|
|
elif cmd == "0104":
|
|
# Engine Load (formula: A * 100 / 255)
|
|
a = int(min(255, max(0, load * 255.0 / 100.0)))
|
|
return f"41 04 {a:02X}\r\r>"
|
|
|
|
elif cmd == "0105":
|
|
# Engine Coolant Temperature (formula: A - 40)
|
|
a = int(min(255, max(0, coolant + 40.0)))
|
|
return f"41 05 {a:02X}\r\r>"
|
|
|
|
elif cmd == "010B":
|
|
# MAP - Manifold Absolute Pressure in psi (formula in code: A * 0.145)
|
|
# So A = psi / 0.145
|
|
a = int(min(255, max(0, map_p / 0.145)))
|
|
return f"41 0B {a:02X}\r\r>"
|
|
|
|
elif cmd == "010C":
|
|
# Engine RPM (formula: (256 * A + B) / 4.0)
|
|
# So 256*A + B = RPM * 4
|
|
val = int(min(65535, max(0, rpm * 4.0)))
|
|
a = (val >> 8) & 0xFF
|
|
b = val & 0xFF
|
|
return f"41 0C {a:02X} {b:02X}\r\r>"
|
|
|
|
elif cmd == "010D":
|
|
# Vehicle Speed in km/h (formula: A)
|
|
a = int(min(255, max(0, speed)))
|
|
return f"41 0D {a:02X}\r\r>"
|
|
|
|
elif cmd == "010F":
|
|
# Intake Air Temp (formula: A - 40)
|
|
a = int(min(255, max(0, coolant - 5.0 + 40.0)))
|
|
return f"41 0F {a:02X}\r\r>"
|
|
|
|
elif cmd == "0142":
|
|
# Control Module Voltage / PCM Volt (formula: (256 * A + B) / 1000.0)
|
|
val = int(min(65535, max(0, volt * 1000.0)))
|
|
a = (val >> 8) & 0xFF
|
|
b = val & 0xFF
|
|
return f"41 42 {a:02X} {b:02X}\r\r>"
|
|
|
|
elif cmd == "015C":
|
|
# Engine Oil Temperature (formula: A - 40)
|
|
a = int(min(255, max(0, oil + 40.0)))
|
|
return f"41 5C {a:02X}\r\r>"
|
|
|
|
elif cmd == "221E1C":
|
|
# Ford T6 Transmission Fluid Temp (formula: (256 * A + B) / 16.0)
|
|
val = int(min(65535, max(0, trans * 16.0)))
|
|
a = (val >> 8) & 0xFF
|
|
b = val & 0xFF
|
|
return f"62 1E 1C {a:02X} {b:02X}\r\r>"
|
|
|
|
elif cmd == "221674":
|
|
# Ford T5 Transmission Temp
|
|
val = int(min(65535, max(0, (trans + 18.0) * 72.0 / 5.0)))
|
|
a = (val >> 8) & 0xFF
|
|
b = val & 0xFF
|
|
return f"62 16 74 {a:02X} {b:02X}\r\r>"
|
|
|
|
elif cmd == "0902":
|
|
# VIN request (Ford format expected by elm327.h)
|
|
# Sample simulated VIN: 1FTFW1ED4NFC12345
|
|
return "014 \r0: 49 02 01 31 46 54 \r1: 46 57 31 45 44 34 4E \r2: 46 43 31 32 33 34 35 \r\r>"
|
|
|
|
elif cmd == "04":
|
|
# Clear DTC
|
|
return "44\r\r>"
|
|
|
|
elif cmd == "03" or cmd == "07" or cmd == "0A":
|
|
# Trouble codes: 43 00 = 0 DTCs
|
|
return "43 00\r\r>"
|
|
|
|
# Fallback
|
|
return "NO DATA\r\r>"
|
|
|
|
|
|
def handle_client_stream(sock):
|
|
"""Handles communication with connected ESP32 CYD"""
|
|
print("[OBD2] ESP32 conectado! Enviando respuestas...")
|
|
buffer = ""
|
|
last_display_time = 0
|
|
|
|
try:
|
|
sock.settimeout(0.5)
|
|
while True:
|
|
try:
|
|
data = sock.recv(256)
|
|
if not data:
|
|
print("[OBD2] Cliente desconectado (EOF).")
|
|
break
|
|
|
|
text = data.decode("latin1", errors="replace")
|
|
buffer += text
|
|
|
|
while "\r" in buffer or "\n" in buffer:
|
|
# Find split point
|
|
r_idx = buffer.find("\r")
|
|
n_idx = buffer.find("\n")
|
|
if r_idx != -1 and n_idx != -1:
|
|
split_idx = min(r_idx, n_idx)
|
|
elif r_idx != -1:
|
|
split_idx = r_idx
|
|
else:
|
|
split_idx = n_idx
|
|
|
|
cmd = buffer[:split_idx].strip()
|
|
buffer = buffer[split_idx+1:]
|
|
|
|
response = format_elm_response(cmd)
|
|
sock.sendall(response.encode("latin1"))
|
|
|
|
now = time.time()
|
|
if now - last_display_time > 0.4:
|
|
last_display_time = now
|
|
cmd_display = cmd if cmd else "<CR>"
|
|
print(f"\r[ECT: {state.coolant_temp:3.0f}°C | RPM: {state.rpm:4.0f} | SPEED: {state.speed:3.0f} km/h | Volt: {state.pcm_volt:4.1f}V | CMD: {cmd_display:6s} -> {response.strip()}]", end="", flush=True)
|
|
|
|
except socket.timeout:
|
|
pass
|
|
except BlockingIOError:
|
|
time.sleep(0.01)
|
|
|
|
except Exception as e:
|
|
print(f"\n[OBD2] Error en socket: {e}")
|
|
finally:
|
|
try:
|
|
sock.close()
|
|
except Exception:
|
|
pass
|
|
print("\n[OBD2] Sesión cerrada.")
|
|
|
|
|
|
def simulation_worker():
|
|
"""Background thread to continuously update simulated values"""
|
|
while True:
|
|
state.update_simulation()
|
|
time.sleep(0.05)
|
|
|
|
|
|
def run_emulator():
|
|
print("=" * 65)
|
|
print(" ESP32 CYD OBD2 Bluetooth Emulator (ELM327 v1.5)")
|
|
print("=" * 65)
|
|
|
|
# Start simulation thread
|
|
sim_thread = threading.Thread(target=simulation_worker, daemon=True)
|
|
sim_thread.start()
|
|
|
|
# Set up D-Bus loop for BlueZ SPP registration
|
|
DBusGMainLoop(set_as_default=True)
|
|
bus = dbus.SystemBus()
|
|
profile_path = "/org/bluez/obd2_profile"
|
|
|
|
profile = SPPProfile(bus, profile_path)
|
|
profile_manager = dbus.Interface(
|
|
bus.get_object("org.bluez", "/org/bluez"),
|
|
"org.bluez.ProfileManager1"
|
|
)
|
|
|
|
opts = {
|
|
"Name": "SerialPort",
|
|
"Channel": dbus.UInt16(1),
|
|
"Role": "server",
|
|
"Service": "00001101-0000-1000-8000-00805F9B34FB",
|
|
"AutoConnect": True,
|
|
}
|
|
|
|
try:
|
|
profile_manager.RegisterProfile(profile_path, "00001101-0000-1000-8000-00805F9B34FB", opts)
|
|
print("[BlueZ] Perfil SPP registrado con éxito.")
|
|
except Exception as e:
|
|
print(f"[BlueZ] Aviso al registrar perfil SPP: {e}")
|
|
|
|
# Register BlueZ Agent for auto-pairing and authorization
|
|
agent_path = "/org/bluez/obd2_agent"
|
|
agent = AutoAgent(bus, agent_path)
|
|
agent_manager = None
|
|
try:
|
|
agent_manager = dbus.Interface(
|
|
bus.get_object("org.bluez", "/org/bluez"),
|
|
"org.bluez.AgentManager1"
|
|
)
|
|
agent_manager.RegisterAgent(agent_path, "NoInputNoOutput")
|
|
agent_manager.RequestDefaultAgent(agent_path)
|
|
print("[BlueZ] Agente de autenticación y emparejamiento registrado.")
|
|
except Exception as e:
|
|
print(f"[BlueZ] Aviso al registrar agente: {e}")
|
|
|
|
# Configure adapter properties (Discoverable, Pairable, Alias)
|
|
try:
|
|
manager = dbus.Interface(bus.get_object("org.bluez", "/"), "org.freedesktop.DBus.ObjectManager")
|
|
objects = manager.GetManagedObjects()
|
|
for path, ifaces in objects.items():
|
|
if "org.bluez.Adapter1" in ifaces:
|
|
props = dbus.Interface(bus.get_object("org.bluez", path), "org.freedesktop.DBus.Properties")
|
|
props.Set("org.bluez.Adapter1", "Alias", dbus.String("OBDII"))
|
|
props.Set("org.bluez.Adapter1", "DiscoverableTimeout", dbus.UInt32(0))
|
|
props.Set("org.bluez.Adapter1", "Discoverable", dbus.Boolean(True))
|
|
props.Set("org.bluez.Adapter1", "Pairable", dbus.Boolean(True))
|
|
print(f"[BlueZ] Adaptador configurado en {path} (Nombre: OBDII, Visible y Emparejable).")
|
|
break
|
|
except Exception as e:
|
|
print(f"[BlueZ] Aviso al configurar adaptador: {e}")
|
|
|
|
# Start GLib main loop in background thread for D-Bus signals and methods
|
|
glib_loop = GLib.MainLoop()
|
|
dbus_thread = threading.Thread(target=glib_loop.run, daemon=True)
|
|
dbus_thread.start()
|
|
|
|
# Create native RFCOMM socket fallback
|
|
server_sock = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
|
|
try:
|
|
server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
server_sock.bind(("00:00:00:00:00:00", 1))
|
|
server_sock.listen(1)
|
|
print("[Bluetooth] Servidor RFCOMM escuchando en canal 1.")
|
|
except Exception as e:
|
|
print(f"[Bluetooth] Servidor RFCOMM: {e}")
|
|
|
|
print("\n[Listo] Esperando conexión de la pantalla CYD...")
|
|
print("Asegúrate de que tu CYD esté encendida buscando 'OBDII'.")
|
|
print("Presiona Ctrl+C para salir.\n")
|
|
|
|
try:
|
|
while True:
|
|
rlist, _, _ = select.select([server_sock], [], [], 1.0)
|
|
if server_sock in rlist:
|
|
client_sock, client_info = server_sock.accept()
|
|
print(f"\n[Bluetooth Socket] Conexión entrante aceptada desde {client_info}!")
|
|
t = threading.Thread(target=handle_client_stream, args=(client_sock,), daemon=True)
|
|
t.start()
|
|
except KeyboardInterrupt:
|
|
print("\nDeteniendo emulador...")
|
|
finally:
|
|
try:
|
|
profile_manager.UnregisterProfile(profile_path)
|
|
except Exception:
|
|
pass
|
|
if agent_manager:
|
|
try:
|
|
agent_manager.UnregisterAgent(agent_path)
|
|
except Exception:
|
|
pass
|
|
server_sock.close()
|
|
print("Emulador finalizado.")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
run_emulator()
|