APB1ENR), RCC_APB1ENR_TIM6EN); // Enable interrupt timer peripheral
}
diff --git a/panda/board/startup_stm32f205xx.s b/panda/board/stm32fx/startup_stm32f205xx.s
similarity index 100%
rename from panda/board/startup_stm32f205xx.s
rename to panda/board/stm32fx/startup_stm32f205xx.s
diff --git a/panda/board/startup_stm32f413xx.s b/panda/board/stm32fx/startup_stm32f413xx.s
similarity index 100%
rename from panda/board/startup_stm32f413xx.s
rename to panda/board/stm32fx/startup_stm32f413xx.s
diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32fx/stm32fx_config.h
new file mode 100644
index 000000000..8bcbfa320
--- /dev/null
+++ b/panda/board/stm32fx/stm32fx_config.h
@@ -0,0 +1,88 @@
+#ifdef STM32F4
+ #include "stm32fx/inc/stm32f4xx.h"
+ #include "stm32fx/inc/stm32f4xx_hal_gpio_ex.h"
+ #define MCU_IDCODE 0x463U
+#else
+ #include "stm32fx/inc/stm32f2xx.h"
+ #include "stm32fx/inc/stm32f2xx_hal_gpio_ex.h"
+ #define MCU_IDCODE 0x411U
+#endif
+// from the linker script
+#define APP_START_ADDRESS 0x8004000U
+
+#define CORE_FREQ 96U // in Mhz
+//APB1 - 48Mhz, APB2 - 96Mhz
+#define APB1_FREQ CORE_FREQ/2U
+#define APB2_FREQ CORE_FREQ/1U
+
+#define BOOTLOADER_ADDRESS 0x1FFF0004U
+
+// Around (1Mbps / 8 bits/byte / 12 bytes per message)
+#define CAN_INTERRUPT_RATE 12000U
+
+#define MAX_LED_FADE 8192U
+
+// Threshold voltage (mV) for either of the SBUs to be below before deciding harness is connected
+#define HARNESS_CONNECTED_THRESHOLD 2500U
+
+#define NUM_INTERRUPTS 102U // There are 102 external interrupt sources (see stm32f413.h)
+
+#define TICK_TIMER_IRQ TIM1_BRK_TIM9_IRQn
+#define TICK_TIMER TIM9
+
+#define MICROSECOND_TIMER TIM2
+
+#define INTERRUPT_TIMER_IRQ TIM6_DAC_IRQn
+#define INTERRUPT_TIMER TIM6
+
+#define PROVISION_CHUNK_ADDRESS 0x1FFF79E0U
+#define DEVICE_SERIAL_NUMBER_ADDRESS 0x1FFF79C0U
+
+#ifndef BOOTSTUB
+ #ifdef PANDA
+ #include "main_declarations.h"
+ #else
+ #include "pedal/main_declarations.h"
+ #endif
+#else
+ #include "bootstub_declarations.h"
+#endif
+
+#include "libc.h"
+#include "critical.h"
+#include "faults.h"
+
+#include "drivers/registers.h"
+#include "drivers/interrupts.h"
+#include "drivers/gpio.h"
+#include "stm32fx/peripherals.h"
+#include "stm32fx/interrupt_handlers.h"
+#include "drivers/timers.h"
+#include "stm32fx/lladc.h"
+#include "stm32fx/board.h"
+#include "stm32fx/clock.h"
+
+#if !defined (BOOTSTUB) && (defined(PANDA) || defined(PEDAL_USB))
+ #include "drivers/uart.h"
+ #include "stm32fx/lluart.h"
+#endif
+
+#ifndef BOOTSTUB
+ #include "stm32fx/llcan.h"
+#endif
+
+#if defined(PANDA) || defined(BOOTSTUB) || defined(PEDAL_USB)
+ #include "stm32fx/llusb.h"
+#endif
+
+#ifdef PEDAL
+ #include "stm32fx/lldac.h"
+#endif
+
+void early_gpio_float(void) {
+ RCC->AHB1ENR = RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN;
+
+ GPIOA->MODER = 0; GPIOB->MODER = 0; GPIOC->MODER = 0;
+ GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0;
+ GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0;
+}
diff --git a/panda/board/stm32_flash.ld b/panda/board/stm32fx/stm32fx_flash.ld
similarity index 97%
rename from panda/board/stm32_flash.ld
rename to panda/board/stm32fx/stm32fx_flash.ld
index 500319377..e6b2ef09d 100644
--- a/panda/board/stm32_flash.ld
+++ b/panda/board/stm32fx/stm32fx_flash.ld
@@ -1,7 +1,7 @@
/*
*****************************************************************************
**
-** File : stm32_flash.ld
+** File : stm32f4_flash.ld
**
** Abstract : Linker script for STM32F407VG Device with
** 1024KByte FLASH, 192KByte RAM
@@ -15,7 +15,7 @@
**
** Environment : Atollic TrueSTUDIO(R)
**
-** Distribution: The file is distributed as is, without any warranty
+** Distribution: The file is distributed "as is," without any warranty
** of any kind.
**
** (c)Copyright Atollic AB.
diff --git a/panda/board/tests/test_rsa.c b/panda/board/tests/test_rsa.c
index f4a7d6be0..5c784e23a 100644
--- a/panda/board/tests/test_rsa.c
+++ b/panda/board/tests/test_rsa.c
@@ -32,4 +32,3 @@ int main() {
return 0;
}
-
diff --git a/panda/examples/query_fw_versions.py b/panda/examples/query_fw_versions.py
new file mode 100755
index 000000000..6795f62c2
--- /dev/null
+++ b/panda/examples/query_fw_versions.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+import argparse
+from tqdm import tqdm
+from panda import Panda
+from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE, DATA_IDENTIFIER_TYPE
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--rxoffset', default="0x8")
+ parser.add_argument('--nonstandard', action='store_true')
+ parser.add_argument('--debug', action='store_true')
+ parser.add_argument('--addr')
+ args = parser.parse_args()
+
+ if args.addr:
+ addrs = [int(args.addr, base=16)]
+ else:
+ addrs = [0x700 + i for i in range(256)]
+ addrs += [0x18da0000 + (i << 8) + 0xf1 for i in range(256)]
+ results = {}
+
+ uds_data_ids = {}
+ for std_id in DATA_IDENTIFIER_TYPE:
+ uds_data_ids[std_id.value] = std_id.name
+ if args.nonstandard:
+ for uds_id in range(0xf100,0xf180):
+ uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_VEHICLE_MANUFACTURER_SPECIFIC_DATA_IDENTIFIER"
+ for uds_id in range(0xf1a0,0xf1f0):
+ uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_VEHICLE_MANUFACTURER_SPECIFIC"
+ for uds_id in range(0xf1f0,0xf200):
+ uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_SYSTEM_SUPPLIER_SPECIFIC"
+
+ panda = Panda()
+ panda.set_safety_mode(Panda.SAFETY_ELM327)
+ panda.set_power_save(0)
+ print("querying addresses ...")
+ with tqdm(addrs) as t:
+ for addr in t:
+ # skip functional broadcast addrs
+ if addr == 0x7df or addr == 0x18db33f1:
+ continue
+ t.set_description(hex(addr))
+
+ uds_client = UdsClient(panda, addr, addr + int(args.rxoffset, base=16), bus=1 if panda.has_obd() else 0, timeout=0.2, debug=args.debug)
+ # Check for anything alive at this address, and switch to the highest
+ # available diagnostic session without security access
+ try:
+ uds_client.tester_present()
+ uds_client.diagnostic_session_control(SESSION_TYPE.DEFAULT)
+ uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC)
+ except NegativeResponseError:
+ pass
+ except MessageTimeoutError:
+ continue
+
+ # Run queries against all standard UDS data identifiers, plus selected
+ # non-standardized identifier ranges if requested
+ resp = {}
+ for uds_data_id in sorted(uds_data_ids):
+ try:
+ data = uds_client.read_data_by_identifier(uds_data_id) # type: ignore
+ if data:
+ resp[uds_data_id] = data
+ except (NegativeResponseError, MessageTimeoutError):
+ pass
+
+ if resp.keys():
+ results[addr] = resp
+
+ if len(results.items()):
+ for addr, resp in results.items():
+ print(f"\n\n*** Results for address 0x{addr:X} ***\n\n")
+ for rid, dat in resp.items():
+ print(f"0x{rid:02X} {uds_data_ids[rid]}: {dat}")
+ else:
+ print("no fw versions found!")
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 1ef4d2a4a..a4b0dbba0 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -138,11 +138,15 @@ class Panda(object):
HW_TYPE_BLACK_PANDA = b'\x03'
HW_TYPE_PEDAL = b'\x04'
HW_TYPE_UNO = b'\x05'
+ HW_TYPE_DOS = b'\x06'
CLOCK_SOURCE_MODE_DISABLED = 0
CLOCK_SOURCE_MODE_FREE_RUNNING = 1
CLOCK_SOURCE_MODE_EXTERNAL_SYNC = 2
+ FLAG_HONDA_ALT_BRAKE = 1
+ FLAG_HONDA_BOSCH_LONG = 2
+
def __init__(self, serial=None, claim=True):
self._serial = serial
self._handle = None
@@ -177,7 +181,6 @@ class Panda(object):
self._serial = this_serial
print("opening device", self._serial, hex(device.getProductID()))
self.bootstub = device.getProductID() == 0xddee
- self.legacy = (device.getbcdDevice() != 0x2300)
self._handle = device.open()
if sys.platform not in ["win32", "cygwin", "msys", "darwin"]:
self._handle.setAutoDetachKernelDriver(True)
@@ -390,11 +393,17 @@ class Panda(object):
def is_black(self):
return self.get_type() == Panda.HW_TYPE_BLACK_PANDA
+ def is_pedal(self):
+ return self.get_type() == Panda.HW_TYPE_PEDAL
+
def is_uno(self):
return self.get_type() == Panda.HW_TYPE_UNO
+ def is_dos(self):
+ return self.get_type() == Panda.HW_TYPE_DOS
+
def has_obd(self):
- return (self.is_uno() or self.is_black())
+ return (self.is_uno() or self.is_dos() or self.is_black())
def get_serial(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
@@ -420,8 +429,10 @@ class Panda(object):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
time.sleep(0.2)
- def set_safety_mode(self, mode=SAFETY_SILENT):
+ def set_safety_mode(self, mode=SAFETY_SILENT, disable_heartbeat=True):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, 0, b'')
+ if disable_heartbeat:
+ self.set_heartbeat_disabled()
def set_can_forwarding(self, from_bus, to_bus):
# TODO: This feature may not work correctly with saturated buses
@@ -619,6 +630,11 @@ class Panda(object):
def send_heartbeat(self):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf3, 0, 0, b'')
+ # disable heartbeat checks for use outside of openpilot
+ # sending a heartbeat will reenable the checks
+ def set_heartbeat_disabled(self):
+ self._handle.controlWrite(Panda.REQUEST_OUT, 0xf8, 0, 0, b'')
+
# ******************* RTC *******************
def set_datetime(self, dt):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xa1, int(dt.year), 0, b'')
diff --git a/panda/python/dfu.py b/panda/python/dfu.py
index f0e055878..e7d1d9151 100644
--- a/panda/python/dfu.py
+++ b/panda/python/dfu.py
@@ -22,7 +22,6 @@ class PandaDFU(object):
continue
if this_dfu_serial == dfu_serial or dfu_serial is None:
self._handle = device.open()
- self.legacy = "07*128Kg" in self._handle.getASCIIStringDescriptor(4)
return
raise Exception("failed to open " + dfu_serial if dfu_serial is not None else "DFU device")
diff --git a/panda/python/serial.py b/panda/python/serial.py
index 93ed2dfe4..97fe8f1d9 100644
--- a/panda/python/serial.py
+++ b/panda/python/serial.py
@@ -4,6 +4,7 @@ class PandaSerial(object):
self.panda = panda
self.port = port
self.panda.set_uart_parity(self.port, 0)
+ self._baudrate = baud
self.panda.set_uart_baud(self.port, baud)
self.buf = b""
@@ -20,3 +21,15 @@ class PandaSerial(object):
def close(self):
pass
+
+ def flush(self):
+ pass
+
+ @property
+ def baudrate(self):
+ return self._baudrate
+
+ @baudrate.setter
+ def baudrate(self, value):
+ self.panda.set_uart_baud(self.port, value)
+ self._baudrate = value
diff --git a/selfdrive/assets/images/network_0.png b/selfdrive/assets/images/network_0.png
deleted file mode 100644
index 2ce959ca5..000000000
Binary files a/selfdrive/assets/images/network_0.png and /dev/null differ
diff --git a/selfdrive/assets/images/network_1.png b/selfdrive/assets/images/network_1.png
deleted file mode 100644
index d7ae713f9..000000000
Binary files a/selfdrive/assets/images/network_1.png and /dev/null differ
diff --git a/selfdrive/assets/images/network_2.png b/selfdrive/assets/images/network_2.png
deleted file mode 100644
index 17ecd977f..000000000
Binary files a/selfdrive/assets/images/network_2.png and /dev/null differ
diff --git a/selfdrive/assets/images/network_3.png b/selfdrive/assets/images/network_3.png
deleted file mode 100644
index 1e854e678..000000000
Binary files a/selfdrive/assets/images/network_3.png and /dev/null differ
diff --git a/selfdrive/assets/images/network_4.png b/selfdrive/assets/images/network_4.png
deleted file mode 100644
index 08c9ab91f..000000000
Binary files a/selfdrive/assets/images/network_4.png and /dev/null differ
diff --git a/selfdrive/assets/images/network_5.png b/selfdrive/assets/images/network_5.png
deleted file mode 100644
index fba67a95a..000000000
Binary files a/selfdrive/assets/images/network_5.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_close.svg b/selfdrive/assets/offroad/icon_close.svg
new file mode 100644
index 000000000..4c063371a
--- /dev/null
+++ b/selfdrive/assets/offroad/icon_close.svg
@@ -0,0 +1,4 @@
+
diff --git a/selfdrive/assets/offroad/icon_lock_closed.svg b/selfdrive/assets/offroad/icon_lock_closed.svg
new file mode 100644
index 000000000..b78709740
--- /dev/null
+++ b/selfdrive/assets/offroad/icon_lock_closed.svg
@@ -0,0 +1,4 @@
+
+
diff --git a/selfdrive/assets/offroad/tc.html b/selfdrive/assets/offroad/tc.html
index 35e791f7f..70d08906f 100644
--- a/selfdrive/assets/offroad/tc.html
+++ b/selfdrive/assets/offroad/tc.html
@@ -4,14 +4,13 @@
openpilot Terms of Service
- openpilot Terms and Conditions
The Terms and Conditions below are effective for all users
Last Updated on October 18, 2019
Please read these Terms of Use (“Terms”) carefully before using openpilot which is open-sourced software developed by Comma.ai, Inc., a corporation organized under the laws of Delaware (“comma,” “us,” “we,” or “our”).
diff --git a/selfdrive/assets/training/step0.png b/selfdrive/assets/training/step0.png
index badeff6c5..c5401136e 100644
Binary files a/selfdrive/assets/training/step0.png and b/selfdrive/assets/training/step0.png differ
diff --git a/selfdrive/assets/training/step1.png b/selfdrive/assets/training/step1.png
index 9fa6cf683..5976ce034 100644
Binary files a/selfdrive/assets/training/step1.png and b/selfdrive/assets/training/step1.png differ
diff --git a/selfdrive/assets/training/step10.png b/selfdrive/assets/training/step10.png
index 10d216c8f..7c9c78130 100644
Binary files a/selfdrive/assets/training/step10.png and b/selfdrive/assets/training/step10.png differ
diff --git a/selfdrive/assets/training/step11.png b/selfdrive/assets/training/step11.png
index 71d98864f..cdfd0fad1 100644
Binary files a/selfdrive/assets/training/step11.png and b/selfdrive/assets/training/step11.png differ
diff --git a/selfdrive/assets/training/step12.png b/selfdrive/assets/training/step12.png
index 627f235ca..1196e0736 100644
Binary files a/selfdrive/assets/training/step12.png and b/selfdrive/assets/training/step12.png differ
diff --git a/selfdrive/assets/training/step13.png b/selfdrive/assets/training/step13.png
index 2430c6c80..eff7ed2e9 100644
Binary files a/selfdrive/assets/training/step13.png and b/selfdrive/assets/training/step13.png differ
diff --git a/selfdrive/assets/training/step14.png b/selfdrive/assets/training/step14.png
index 835d6e24f..f1975cc0b 100644
Binary files a/selfdrive/assets/training/step14.png and b/selfdrive/assets/training/step14.png differ
diff --git a/selfdrive/assets/training/step15.png b/selfdrive/assets/training/step15.png
index c65b0e2af..3479764e2 100644
Binary files a/selfdrive/assets/training/step15.png and b/selfdrive/assets/training/step15.png differ
diff --git a/selfdrive/assets/training/step16.png b/selfdrive/assets/training/step16.png
index bff559846..a1f2ac263 100644
Binary files a/selfdrive/assets/training/step16.png and b/selfdrive/assets/training/step16.png differ
diff --git a/selfdrive/assets/training/step17.png b/selfdrive/assets/training/step17.png
index 81d214d0a..b35a17374 100644
Binary files a/selfdrive/assets/training/step17.png and b/selfdrive/assets/training/step17.png differ
diff --git a/selfdrive/assets/training/step2.png b/selfdrive/assets/training/step2.png
index ab1498bcc..58003315d 100644
Binary files a/selfdrive/assets/training/step2.png and b/selfdrive/assets/training/step2.png differ
diff --git a/selfdrive/assets/training/step3.png b/selfdrive/assets/training/step3.png
index d19e388ed..e31044b85 100644
Binary files a/selfdrive/assets/training/step3.png and b/selfdrive/assets/training/step3.png differ
diff --git a/selfdrive/assets/training/step4.png b/selfdrive/assets/training/step4.png
index 874b19390..57e5e234f 100644
Binary files a/selfdrive/assets/training/step4.png and b/selfdrive/assets/training/step4.png differ
diff --git a/selfdrive/assets/training/step5.png b/selfdrive/assets/training/step5.png
index 9ee3388c3..af9fbd58c 100644
Binary files a/selfdrive/assets/training/step5.png and b/selfdrive/assets/training/step5.png differ
diff --git a/selfdrive/assets/training/step6.png b/selfdrive/assets/training/step6.png
index 14463ea04..c8ca6b526 100644
Binary files a/selfdrive/assets/training/step6.png and b/selfdrive/assets/training/step6.png differ
diff --git a/selfdrive/assets/training/step7.png b/selfdrive/assets/training/step7.png
index 682333214..28af33b91 100644
Binary files a/selfdrive/assets/training/step7.png and b/selfdrive/assets/training/step7.png differ
diff --git a/selfdrive/assets/training/step8.png b/selfdrive/assets/training/step8.png
index 1a5bb1367..615a80922 100644
Binary files a/selfdrive/assets/training/step8.png and b/selfdrive/assets/training/step8.png differ
diff --git a/selfdrive/assets/training/step9.png b/selfdrive/assets/training/step9.png
index f9b53cf7a..f8ed28da5 100644
Binary files a/selfdrive/assets/training/step9.png and b/selfdrive/assets/training/step9.png differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 1659b692c..720fd8926 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -54,7 +54,7 @@ def handle_long_poll(ws):
threads = [
threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'),
- threading.Thread(target=ws_send, args=(ws, end_event), name='wc_send'),
+ threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'),
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
threading.Thread(target=log_handler, args=(end_event,), name='log_handler'),
] + [
@@ -116,7 +116,7 @@ def _do_upload(upload_item):
return requests.put(upload_item.url,
data=f,
headers={**upload_item.headers, 'Content-Length': str(size)},
- timeout=10)
+ timeout=30)
# security: user should be able to request any message from their car
@@ -267,6 +267,11 @@ def getNetworkType():
return HARDWARE.get_network_type()
+@dispatcher.add_method
+def getNetworks():
+ return HARDWARE.get_networks()
+
+
@dispatcher.add_method
def takeSnapshot():
from selfdrive.camerad.snapshot.snapshot import snapshot, jpeg_write
@@ -424,7 +429,7 @@ def ws_recv(ws, end_event):
except WebSocketTimeoutException:
ns_since_last_ping = int(sec_since_boot() * 1e9) - last_ping
if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9:
- cloudlog.exception("athenad.wc_recv.timeout")
+ cloudlog.exception("athenad.ws_recv.timeout")
end_event.set()
except Exception:
cloudlog.exception("athenad.ws_recv.exception")
@@ -479,8 +484,7 @@ def main():
ws = create_connection(ws_uri,
cookie="jwt=" + api.get_token(),
enable_multithread=True,
- timeout=1.0)
- ws.settimeout(1)
+ timeout=30.0)
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
manage_tokens(api)
@@ -492,6 +496,8 @@ def main():
except (ConnectionError, TimeoutError, WebSocketException):
conn_retries += 1
params.delete("LastAthenaPingTime")
+ if TICI:
+ cloudlog.exception("athenad.main.exception2")
except Exception:
cloudlog.exception("athenad.main.exception")
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index e52c8d2e6..c770845af 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -1,8 +1,4 @@
-#include
#include
-#include
-#include
-#include
#include
#include
#include
@@ -12,6 +8,10 @@
#include
#include
#include
+#include
+#include
+#include
+#include
#include
#include
@@ -51,7 +51,7 @@ void safety_setter_thread() {
// switch to SILENT when CarVin param is read
while (true) {
- if (do_exit || !panda->connected){
+ if (do_exit || !panda->connected) {
safety_setter_thread_running = false;
return;
};
@@ -67,12 +67,12 @@ void safety_setter_thread() {
}
// VIN query done, stop listening to OBDII
- panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+ panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
std::string params;
LOGW("waiting for params to set safety model");
while (true) {
- if (do_exit || !panda->connected){
+ if (do_exit || !panda->connected) {
safety_setter_thread_running = false;
return;
};
@@ -124,7 +124,7 @@ bool usb_connect() {
// Convert to hex for offroad
char fw_sig_hex_buf[16] = {0};
const uint8_t *fw_sig_buf = fw_sig->data();
- for (size_t i = 0; i < 8; i++){
+ for (size_t i = 0; i < 8; i++) {
fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4);
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF);
}
@@ -146,7 +146,7 @@ bool usb_connect() {
}
#endif
- if (tmp_panda->has_rtc){
+ if (tmp_panda->has_rtc) {
setenv("TZ","UTC",1);
struct tm sys_time = util::get_time();
struct tm rtc_time = tmp_panda->get_rtc();
@@ -199,7 +199,7 @@ void can_send_thread(bool fake_send) {
while (!do_exit && panda->connected) {
Message * msg = subscriber->receive();
- if (!msg){
+ if (!msg) {
if (errno == EINTR) {
do_exit = true;
}
@@ -211,7 +211,7 @@ void can_send_thread(bool fake_send) {
//Dont send if older than 1 second
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
- if (!fake_send){
+ if (!fake_send) {
panda->can_send(event.getSendcan());
}
}
@@ -241,7 +241,7 @@ void can_recv_thread() {
if (remaining > 0) {
std::this_thread::sleep_for(std::chrono::nanoseconds(remaining));
} else {
- if (ignition){
+ if (ignition) {
LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining);
}
next_frame_time = cur_time;
@@ -292,7 +292,7 @@ void panda_state_thread(bool spoofing_started) {
#ifndef __x86_64__
bool power_save_desired = !ignition;
- if (pandaState.power_save_enabled != power_save_desired){
+ if (pandaState.power_save_enabled != power_save_desired) {
panda->set_power_saving(power_save_desired);
}
@@ -317,12 +317,12 @@ void panda_state_thread(bool spoofing_started) {
}
// Write to rtc once per minute when no ignition present
- if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){
+ if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)) {
// Write time to RTC if it looks reasonable
setenv("TZ","UTC",1);
struct tm sys_time = util::get_time();
- if (util::time_valid(sys_time)){
+ if (util::time_valid(sys_time)) {
struct tm rtc_time = panda->get_rtc();
double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
@@ -388,7 +388,7 @@ void panda_state_thread(bool spoofing_started) {
size_t i = 0;
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
- f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TIM9); f++){
+ f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
if (fault_bits.test(f)) {
faults.set(i, cereal::PandaState::FaultType(f));
i++;
@@ -415,11 +415,11 @@ void hardware_control_thread() {
cnt++;
sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update?
- if (!Hardware::PC() && sm.updated("deviceState")){
+ if (!Hardware::PC() && sm.updated("deviceState")) {
// Charging mode
bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled();
- if (charging_disabled != prev_charging_disabled){
- if (charging_disabled){
+ if (charging_disabled != prev_charging_disabled) {
+ if (charging_disabled) {
panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CLIENT);
LOGW("TURN OFF CHARGING!\n");
} else {
@@ -432,15 +432,15 @@ void hardware_control_thread() {
// Other pandas don't have fan/IR to control
if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue;
- if (sm.updated("deviceState")){
+ if (sm.updated("deviceState")) {
// Fan speed
uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired();
- if (fan_speed != prev_fan_speed || cnt % 100 == 0){
+ if (fan_speed != prev_fan_speed || cnt % 100 == 0) {
panda->set_fan_speed(fan_speed);
prev_fan_speed = fan_speed;
}
}
- if (sm.updated("driverCameraState")){
+ if (sm.updated("driverCameraState")) {
auto event = sm["driverCameraState"];
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
last_front_frame_t = event.getLogMonoTime();
@@ -455,11 +455,11 @@ void hardware_control_thread() {
}
// Disable ir_pwr on front frame timeout
uint64_t cur_t = nanos_since_boot();
- if (cur_t - last_front_frame_t > 1e9){
+ if (cur_t - last_front_frame_t > 1e9) {
ir_pwr = 0;
}
- if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){
+ if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0) {
panda->set_ir_pwr(ir_pwr);
prev_ir_pwr = ir_pwr;
}
@@ -492,10 +492,10 @@ void pigeon_thread() {
// Parse message header
if (ignition && recv.length() >= 3) {
- if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2){
+ if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) {
const char msg_cls = recv[2];
uint64_t t = nanos_since_boot();
- if (t > last_recv_time[msg_cls]){
+ if (t > last_recv_time[msg_cls]) {
last_recv_time[msg_cls] = t;
}
}
@@ -512,12 +512,12 @@ void pigeon_thread() {
}
// Check based on null bytes
- if (ignition && recv.length() > 0 && recv[0] == (char)0x00){
+ if (ignition && recv.length() > 0 && recv[0] == (char)0x00) {
need_reset = true;
LOGW("received invalid ublox message while onroad, resetting panda GPS");
}
- if (recv.length() > 0){
+ if (recv.length() > 0) {
pigeon_publish_raw(pm, recv);
}
@@ -559,7 +559,7 @@ int main() {
err = set_core_affinity(Hardware::TICI() ? 4 : 3);
LOG("set affinity returns %d", err);
- while (!do_exit){
+ while (!do_exit) {
std::vector threads;
threads.push_back(std::thread(panda_state_thread, getenv("STARTED") != nullptr));
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 1b563b53d..d75585065 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -11,7 +11,7 @@
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
-Panda::Panda(){
+Panda::Panda() {
// init libusb
int err = libusb_init(&ctx);
if (err != 0) { goto fail; }
@@ -50,14 +50,14 @@ fail:
throw std::runtime_error("Error connecting to panda");
}
-Panda::~Panda(){
+Panda::~Panda() {
std::lock_guard lk(usb_lock);
cleanup();
connected = false;
}
-void Panda::cleanup(){
- if (dev_handle){
+void Panda::cleanup() {
+ if (dev_handle) {
libusb_release_interface(dev_handle, 0);
libusb_close(dev_handle);
}
@@ -80,7 +80,7 @@ int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigne
int err;
const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
- if (!connected){
+ if (!connected) {
return LIBUSB_ERROR_NO_DEVICE;
}
@@ -97,7 +97,7 @@ int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned
int err;
const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
- if (!connected){
+ if (!connected) {
return LIBUSB_ERROR_NO_DEVICE;
}
@@ -114,7 +114,7 @@ int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int lengt
int err;
int transferred = 0;
- if (!connected){
+ if (!connected) {
return 0;
}
@@ -139,7 +139,7 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length
int err;
int transferred = 0;
- if (!connected){
+ if (!connected) {
return 0;
}
@@ -162,7 +162,7 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length
return transferred;
}
-void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param){
+void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param) {
usb_write(0xdc, (uint16_t)safety_model, safety_param);
}
@@ -177,7 +177,7 @@ cereal::PandaState::PandaType Panda::get_hw_type() {
return (cereal::PandaState::PandaType)(hw_query[0]);
}
-void Panda::set_rtc(struct tm sys_time){
+void Panda::set_rtc(struct tm sys_time) {
// tm struct has year defined as years since 1900
usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0);
usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0);
@@ -188,7 +188,7 @@ void Panda::set_rtc(struct tm sys_time){
usb_write(0xa7, (uint16_t)sys_time.tm_sec, 0);
}
-struct tm Panda::get_rtc(){
+struct tm Panda::get_rtc() {
struct __attribute__((packed)) timestamp_t {
uint16_t year; // Starts at 0
uint8_t month;
@@ -212,11 +212,11 @@ struct tm Panda::get_rtc(){
return new_time;
}
-void Panda::set_fan_speed(uint16_t fan_speed){
+void Panda::set_fan_speed(uint16_t fan_speed) {
usb_write(0xb1, fan_speed, 0);
}
-uint16_t Panda::get_fan_speed(){
+uint16_t Panda::get_fan_speed() {
uint16_t fan_speed_rpm = 0;
usb_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm));
return fan_speed_rpm;
@@ -226,17 +226,17 @@ void Panda::set_ir_pwr(uint16_t ir_pwr) {
usb_write(0xb0, ir_pwr, 0);
}
-health_t Panda::get_state(){
+health_t Panda::get_state() {
health_t health {0};
usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
return health;
}
-void Panda::set_loopback(bool loopback){
+void Panda::set_loopback(bool loopback) {
usb_write(0xe5, loopback, 0);
}
-std::optional> Panda::get_firmware_version(){
+std::optional> Panda::get_firmware_version() {
std::vector fw_sig_buf(128);
int read_1 = usb_read(0xd3, 0, 0, &fw_sig_buf[0], 64);
int read_2 = usb_read(0xd4, 0, 0, &fw_sig_buf[64], 64);
@@ -249,19 +249,19 @@ std::optional Panda::get_serial() {
return err >= 0 ? std::make_optional(serial_buf) : std::nullopt;
}
-void Panda::set_power_saving(bool power_saving){
+void Panda::set_power_saving(bool power_saving) {
usb_write(0xe7, power_saving, 0);
}
-void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode){
+void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode) {
usb_write(0xe6, (uint16_t)power_mode, 0);
}
-void Panda::send_heartbeat(){
+void Panda::send_heartbeat() {
usb_write(0xf3, 1, 0);
}
-void Panda::can_send(capnp::List::Reader can_data_list){
+void Panda::can_send(capnp::List::Reader can_data_list) {
static std::vector send;
const int msg_count = can_data_list.size();
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
index dc6a5e49a..002288c6e 100644
--- a/selfdrive/boardd/pigeon.cc
+++ b/selfdrive/boardd/pigeon.cc
@@ -1,11 +1,11 @@
#include "selfdrive/boardd/pigeon.h"
-#include
#include
#include
#include
#include
+#include
#include
#include "selfdrive/common/gpio.h"
@@ -27,26 +27,26 @@ const std::string nack = "\xb5\x62\x05\x00\x02\x00";
const std::string sos_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00";
const std::string sos_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00";
-Pigeon * Pigeon::connect(Panda * p){
+Pigeon * Pigeon::connect(Panda * p) {
PandaPigeon * pigeon = new PandaPigeon();
pigeon->connect(p);
return pigeon;
}
-Pigeon * Pigeon::connect(const char * tty){
+Pigeon * Pigeon::connect(const char * tty) {
TTYPigeon * pigeon = new TTYPigeon();
pigeon->connect(tty);
return pigeon;
}
-bool Pigeon::wait_for_ack(std::string ack, std::string nack){
+bool Pigeon::wait_for_ack(const std::string &ack, const std::string &nack) {
std::string s;
- while (!do_exit){
+ while (!do_exit) {
s += receive();
- if (s.find(ack) != std::string::npos){
+ if (s.find(ack) != std::string::npos) {
LOGD("Received ACK from ublox");
return true;
} else if (s.find(nack) != std::string::npos) {
@@ -62,17 +62,17 @@ bool Pigeon::wait_for_ack(std::string ack, std::string nack){
return false;
}
-bool Pigeon::wait_for_ack(){
+bool Pigeon::wait_for_ack() {
return wait_for_ack(ack, nack);
}
-bool Pigeon::send_with_ack(std::string cmd){
+bool Pigeon::send_with_ack(const std::string &cmd) {
send(cmd);
return wait_for_ack();
}
void Pigeon::init() {
- for (int i = 0; i < 10; i++){
+ for (int i = 0; i < 10; i++) {
if (do_exit) return;
LOGW("panda GPS start");
@@ -118,7 +118,7 @@ void Pigeon::init() {
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
auto time = util::get_time();
- if (util::time_valid(time)){
+ if (util::time_valid(time)) {
LOGW("Sending current time to ublox");
send(ublox::build_ubx_mga_ini_time_utc(time));
}
@@ -129,7 +129,7 @@ void Pigeon::init() {
LOGE("failed to initialize panda GPS");
}
-void Pigeon::stop(){
+void Pigeon::stop() {
LOGW("Storing almanac in ublox flash");
// Controlled GNSS stop
@@ -172,7 +172,7 @@ std::string PandaPigeon::receive() {
std::string r;
r.reserve(0x1000 + 0x40);
unsigned char dat[0x40];
- while (r.length() < 0x1000){
+ while (r.length() < 0x1000) {
int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat));
if (len <= 0) break;
r.append((char*)dat, len);
@@ -185,7 +185,7 @@ void PandaPigeon::set_power(bool power) {
panda->usb_write(0xd9, power, 0);
}
-PandaPigeon::~PandaPigeon(){
+PandaPigeon::~PandaPigeon() {
}
void handle_tty_issue(int err, const char func[]) {
@@ -194,7 +194,7 @@ void handle_tty_issue(int err, const char func[]) {
void TTYPigeon::connect(const char * tty) {
pigeon_tty_fd = open(tty, O_RDWR);
- if (pigeon_tty_fd < 0){
+ if (pigeon_tty_fd < 0) {
handle_tty_issue(errno, __func__);
assert(pigeon_tty_fd >= 0);
}
@@ -222,9 +222,9 @@ void TTYPigeon::connect(const char * tty) {
assert(err == 0);
}
-void TTYPigeon::set_baud(int baud){
+void TTYPigeon::set_baud(int baud) {
speed_t baud_const = 0;
- switch(baud){
+ switch(baud) {
case 9600:
baud_const = B9600;
break;
@@ -264,11 +264,11 @@ std::string TTYPigeon::receive() {
std::string r;
r.reserve(0x1000 + 0x40);
char dat[0x40];
- while (r.length() < 0x1000){
+ while (r.length() < 0x1000) {
int len = read(pigeon_tty_fd, dat, sizeof(dat));
if(len < 0) {
handle_tty_issue(len, __func__);
- } else if (len == 0){
+ } else if (len == 0) {
break;
} else {
r.append(dat, len);
@@ -278,7 +278,7 @@ std::string TTYPigeon::receive() {
return r;
}
-void TTYPigeon::set_power(bool power){
+void TTYPigeon::set_power(bool power) {
#ifdef QCOM2
int err = 0;
err += gpio_init(GPIO_UBLOX_RST_N, true);
@@ -292,6 +292,6 @@ void TTYPigeon::set_power(bool power){
#endif
}
-TTYPigeon::~TTYPigeon(){
+TTYPigeon::~TTYPigeon() {
close(pigeon_tty_fd);
}
diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h
index ff22042dc..ecd42cb7b 100644
--- a/selfdrive/boardd/pigeon.h
+++ b/selfdrive/boardd/pigeon.h
@@ -16,8 +16,8 @@ class Pigeon {
void init();
void stop();
bool wait_for_ack();
- bool wait_for_ack(std::string ack, std::string nack);
- bool send_with_ack(std::string cmd);
+ bool wait_for_ack(const std::string &ack, const std::string &nack);
+ bool send_with_ack(const std::string &cmd);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0;
virtual std::string receive() = 0;
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index cf629bde3..125ac67a9 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -1,8 +1,9 @@
#include "selfdrive/camerad/cameras/camera_common.h"
-#include
-#include
#include
+
+#include
+#include
#include
#include
@@ -124,8 +125,6 @@ bool CameraBuf::acquire() {
const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)};
const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE};
CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0));
- int ggain = camera_state->analog_gain + 4*camera_state->dc_gain_enabled;
- CL_CHECK(clSetKernelArg(krnl_debayer, 3, sizeof(int), &ggain));
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize,
0, 0, &debayer_event));
#else
@@ -162,7 +161,7 @@ bool CameraBuf::acquire() {
}
void CameraBuf::release() {
- if (release_callback){
+ if (release_callback) {
release_callback((void*)camera_state, cur_buf_idx);
}
}
@@ -179,12 +178,14 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
framed.setTimestampSof(frame_data.timestamp_sof);
framed.setFrameLength(frame_data.frame_length);
framed.setIntegLines(frame_data.integ_lines);
- framed.setGlobalGain(frame_data.global_gain);
+ framed.setGain(frame_data.gain);
+ framed.setHighConversionGain(frame_data.high_conversion_gain);
+ framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
+ framed.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setLensPos(frame_data.lens_pos);
framed.setLensSag(frame_data.lens_sag);
framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos);
- framed.setGainFrac(frame_data.gain_frac);
}
kj::Array get_frame_image(const CameraBuf *b) {
@@ -275,39 +276,30 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
free(thumbnail_buffer);
}
-float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted) {
- const uint8_t *pix_ptr = b->cur_yuv_buf->y;
+float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
+ int lum_med;
uint32_t lum_binning[256] = {0};
+ const uint8_t *pix_ptr = b->cur_yuv_buf->y;
+
unsigned int lum_total = 0;
for (int y = y_start; y < y_end; y += y_skip) {
for (int x = x_start; x < x_end; x += x_skip) {
uint8_t lum = pix_ptr[(y * b->rgb_width) + x];
- if (hist_ceil && lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
- continue;
- }
lum_binning[lum]++;
lum_total += 1;
}
}
+
+ // Find mean lumimance value
unsigned int lum_cur = 0;
- int lum_med = 0;
- int lum_med_alt = 0;
- for (lum_med=255; lum_med>=0; lum_med--) {
+ for (lum_med = 255; lum_med >= 0; lum_med--) {
lum_cur += lum_binning[lum_med];
- if (hl_weighted) {
- int lum_med_tmp = 0;
- int hb = HLC_THRESH + (10 - analog_gain);
- if (lum_cur > 0 && lum_med > hb) {
- lum_med_tmp = (lum_med - hb) + 100;
- }
- lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
- }
+
if (lum_cur >= lum_total / 2) {
break;
}
}
- lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*abs(lum_med_alt - lum_med)/lum_total:lum_med;
return lum_med / 256.0;
}
@@ -350,18 +342,12 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;};
const CameraBuf *b = &c->buf;
- bool hist_ceil = false, hl_weighted = false;
int x_offset = 0, y_offset = 0;
int frame_width = b->rgb_width, frame_height = b->rgb_height;
-#ifndef QCOM2
- int analog_gain = -1;
-#else
- int analog_gain = c->analog_gain;
-#endif
+
ExpRect def_rect;
if (Hardware::TICI()) {
- hist_ceil = hl_weighted = true;
x_offset = 630, y_offset = 156;
frame_width = 668, frame_height = frame_width / 1.33;
def_rect = {96, 1832, 2, 242, 1148, 4};
@@ -385,7 +371,7 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
}
}
- camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip, analog_gain, hist_ceil, hl_weighted));
+ camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip));
}
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) {
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index ab3efce28..8bf4ff336 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -1,8 +1,7 @@
#pragma once
-#include
-#include
-
+#include
+#include
#include
#include
@@ -28,7 +27,7 @@
#define CAMERA_ID_MAX 9
#define UI_BUF_COUNT 4
-#define YUV_COUNT 40
+#define YUV_COUNT 100
#define LOG_CAMERA_ID_FCAMERA 0
#define LOG_CAMERA_ID_DCAMERA 1
#define LOG_CAMERA_ID_ECAMERA 2
@@ -68,16 +67,24 @@ typedef struct LogCameraInfo {
typedef struct FrameMetadata {
uint32_t frame_id;
+ unsigned int frame_length;
+
+ // Timestamps
uint64_t timestamp_sof; // only set on tici
uint64_t timestamp_eof;
- unsigned int frame_length;
+
+ // Exposure
unsigned int integ_lines;
- unsigned int global_gain;
+ bool high_conversion_gain;
+ float gain;
+ float measured_grey_fraction;
+ float target_grey_fraction;
+
+ // Focus
unsigned int lens_pos;
float lens_sag;
float lens_err;
float lens_true_pos;
- float gain_frac;
} FrameMetadata;
typedef struct CameraExpInfo {
@@ -128,7 +135,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data);
kj::Array get_frame_image(const CameraBuf *b);
-float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted);
+float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc
index a03feb132..8e6c69e36 100644
--- a/selfdrive/camerad/cameras/camera_frame_stream.cc
+++ b/selfdrive/camerad/cameras/camera_frame_stream.cc
@@ -50,15 +50,13 @@ void run_frame_stream(CameraState &camera, const char* frame_pkt) {
size_t buf_idx = 0;
while (!do_exit) {
sm.update(1000);
- if(sm.updated(frame_pkt)){
+ if(sm.updated(frame_pkt)) {
auto msg = static_cast(sm[frame_pkt]);
auto frame = msg.get(frame_pkt).as();
camera.buf.camera_bufs_metadata[buf_idx] = {
.frame_id = frame.get("frameId").as(),
.timestamp_eof = frame.get("timestampEof").as(),
- .frame_length = frame.get("frameLength").as(),
- .integ_lines = frame.get("integLines").as(),
- .global_gain = frame.get("globalGain").as(),
+ .timestamp_sof = frame.get("timestampSof").as(),
};
cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q;
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index aa386e5c8..7b350215d 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -1,15 +1,15 @@
#include "selfdrive/camerad/cameras/camera_qcom.h"
-#include
#include
-#include
#include
-#include
#include
#include
#include
#include
+#include
+#include
+#include
#include
#include
@@ -282,6 +282,12 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
static void do_autoexposure(CameraState *s, float grey_frac) {
const float target_grey = 0.3;
+
+ s->frame_info_lock.lock();
+ s->measured_grey_fraction = grey_frac;
+ s->target_grey_fraction = target_grey;
+ s->frame_info_lock.unlock();
+
if (s->apply_exposure == ov8865_apply_exposure) {
// gain limits downstream
const float gain_frac_min = 0.015625;
@@ -848,7 +854,7 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
static std::optional get_accel_z(SubMaster *sm) {
sm->update(0);
- if(sm->updated("sensorEvents")){
+ if(sm->updated("sensorEvents")) {
for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
if (event.which() == cereal::SensorEventData::ACCELERATION) {
if (auto v = event.getAcceleration().getV(); v.size() >= 3)
@@ -1107,7 +1113,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if (cnt % 3 == 0) {
const int x = 290, y = 322, width = 560, height = 314;
const int skip = 1;
- camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip, -1, false, false));
+ camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip));
}
}
@@ -1162,12 +1168,14 @@ void cameras_run(MultiCameraState *s) {
.timestamp_eof = timestamp,
.frame_length = (uint32_t)c->frame_length,
.integ_lines = (uint32_t)c->cur_integ_lines,
- .global_gain = (uint32_t)c->cur_gain,
.lens_pos = c->cur_lens_pos,
.lens_sag = c->last_sag_acc_z,
.lens_err = c->focus_err,
.lens_true_pos = c->lens_true_pos,
- .gain_frac = c->cur_gain_frac,
+ .gain = c->cur_gain_frac,
+ .measured_grey_fraction = c->measured_grey_fraction,
+ .target_grey_fraction = c->target_grey_fraction,
+ .high_conversion_gain = false,
};
c->frame_metadata_idx = (c->frame_metadata_idx + 1) % METADATA_BUF_COUNT;
diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h
index 1bcfed205..207857e05 100644
--- a/selfdrive/camerad/cameras/camera_qcom.h
+++ b/selfdrive/camerad/cameras/camera_qcom.h
@@ -1,8 +1,7 @@
#pragma once
-#include
-
#include
+#include
#include
#include "cereal/messaging/messaging.h"
@@ -67,6 +66,10 @@ typedef struct CameraState {
unsigned int max_gain;
float cur_exposure_frac, cur_gain_frac;
int cur_gain, cur_integ_lines;
+
+ float measured_grey_fraction;
+ float target_grey_fraction;
+
std::atomic digital_gain;
camera_apply_exposure_func apply_exposure;
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
deleted file mode 100644
index 44f77d80b..000000000
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ /dev/null
@@ -1,1138 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "media/cam_defs.h"
-#include "media/cam_isp.h"
-#include "media/cam_isp_ife.h"
-#include "media/cam_sensor_cmn_header.h"
-#include "media/cam_sensor.h"
-#include "media/cam_sync.h"
-#include "sensor2_i2c.h"
-
-#include "selfdrive/common/swaglog.h"
-#include "selfdrive/camerad/cameras/camera_qcom2.h"
-
-#define FRAME_WIDTH 1928
-#define FRAME_HEIGHT 1208
-//#define FRAME_STRIDE 1936 // for 8 bit output
-#define FRAME_STRIDE 2416 // for 10 bit output
-
-#define MIPI_SETTLE_CNT 33 // Calculated by camera_freqs.py
-
-extern ExitHandler do_exit;
-
-// global var for AE ops
-std::atomic cam_exp[3] = {{{0}}};
-
-CameraInfo cameras_supported[CAMERA_ID_MAX] = {
- [CAMERA_ID_AR0231] = {
- .frame_width = FRAME_WIDTH,
- .frame_height = FRAME_HEIGHT,
- .frame_stride = FRAME_STRIDE,
- .bayer = true,
- .bayer_flip = 1,
- .hdr = false
- },
-};
-
-float sensor_analog_gains[ANALOG_GAIN_MAX_IDX] = {3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0,
- 5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0,
- 7.0/2.0, 8.0/2.0};
-
-// ************** low level camera helpers ****************
-
-int cam_control(int fd, int op_code, void *handle, int size) {
- struct cam_control camcontrol = {0};
- camcontrol.op_code = op_code;
- camcontrol.handle = (uint64_t)handle;
- if (size == 0) { camcontrol.size = 8;
- camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE;
- } else {
- camcontrol.size = size;
- camcontrol.handle_type = CAM_HANDLE_USER_POINTER;
- }
-
- int ret = ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol);
- if (ret == -1) {
- printf("OP CODE ERR - %d \n", op_code);
- perror("wat");
- }
- return ret;
-}
-
-int device_control(int fd, int op_code, int session_handle, int dev_handle) {
- // start stop and release are all the same
- static struct cam_release_dev_cmd release_dev_cmd;
- release_dev_cmd.session_handle = session_handle;
- release_dev_cmd.dev_handle = dev_handle;
- return cam_control(fd, op_code, &release_dev_cmd, sizeof(release_dev_cmd));
-}
-
-void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
- int mmu_hdl = 0, int mmu_hdl2 = 0) {
- struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0};
- mem_mgr_alloc_cmd.len = len;
- mem_mgr_alloc_cmd.align = align;
- mem_mgr_alloc_cmd.flags = flags;
- mem_mgr_alloc_cmd.num_hdl = 0;
- if (mmu_hdl != 0) {
- mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl;
- mem_mgr_alloc_cmd.num_hdl++;
- }
- if (mmu_hdl2 != 0) {
- mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2;
- mem_mgr_alloc_cmd.num_hdl++;
- }
-
- cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
- *handle = mem_mgr_alloc_cmd.out.buf_handle;
-
- void *ptr = NULL;
- if (mem_mgr_alloc_cmd.out.fd > 0) {
- ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0);
- assert(ptr != MAP_FAILED);
- }
-
- // LOGD("alloced: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr);
-
- return ptr;
-}
-
-void release(int video0_fd, uint32_t handle) {
- int ret;
- struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
- mem_mgr_release_cmd.buf_handle = handle;
-
- ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
- assert(ret == 0);
-}
-
-void release_fd(int video0_fd, uint32_t handle) {
- // handle to fd
- close(handle>>16);
- release(video0_fd, handle);
-}
-
-void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
- struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
- req_mgr_flush_request.session_hdl = session_hdl;
- req_mgr_flush_request.link_hdl = link_hdl;
- req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
- int ret;
- ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
- // LOGD("flushed all req: %d", ret);
-}
-
-// ************** high level camera helpers ****************
-
-void sensors_poke(struct CameraState *s, int request_id) {
- uint32_t cam_packet_handle = 0;
- int size = sizeof(struct cam_packet);
- struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
- pkt->num_cmd_buf = 0;
- pkt->kmd_cmd_buf_index = -1;
- pkt->header.size = size;
- pkt->header.op_code = 0x7f;
- pkt->header.request_id = request_id;
-
- struct cam_config_dev_cmd config_dev_cmd = {};
- config_dev_cmd.session_handle = s->session_handle;
- config_dev_cmd.dev_handle = s->sensor_dev_handle;
- config_dev_cmd.offset = 0;
- config_dev_cmd.packet_handle = cam_packet_handle;
-
- int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
- assert(ret == 0);
-
- munmap(pkt, size);
- release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
-}
-
-void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
- // LOGD("sensors_i2c: %d", len);
- uint32_t cam_packet_handle = 0;
- int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
- struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
- pkt->num_cmd_buf = 1;
- pkt->kmd_cmd_buf_index = -1;
- pkt->header.size = size;
- pkt->header.op_code = op_code;
- struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
-
- buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
- buf_desc[0].type = CAM_CMD_BUF_I2C;
-
- struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
- i2c_random_wr->header.count = len;
- i2c_random_wr->header.op_code = 1;
- i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
- i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD;
- i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
- memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
-
- struct cam_config_dev_cmd config_dev_cmd = {};
- config_dev_cmd.session_handle = s->session_handle;
- config_dev_cmd.dev_handle = s->sensor_dev_handle;
- config_dev_cmd.offset = 0;
- config_dev_cmd.packet_handle = cam_packet_handle;
-
- int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
- assert(ret == 0);
-
- munmap(i2c_random_wr, buf_desc[0].size);
- release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
- munmap(pkt, size);
- release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
-}
-static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
- cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
- unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
- unconditional_wait->delay = delay_ms;
- unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
- return (struct cam_cmd_power *)(unconditional_wait + 1);
-};
-
-void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
- uint32_t cam_packet_handle = 0;
- int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
- struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
- pkt->num_cmd_buf = 2;
- pkt->kmd_cmd_buf_index = -1;
- pkt->header.op_code = 0x1000003;
- pkt->header.size = size;
- struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
-
- buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe);
- buf_desc[0].type = CAM_CMD_BUF_LEGACY;
- struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc_w_mmu_hdl(video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
- struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info);
-
- switch (camera_num) {
- case 0:
- // port 0
- i2c_info->slave_addr = 0x20;
- probe->camera_id = 0;
- break;
- case 1:
- // port 1
- i2c_info->slave_addr = 0x30;
- probe->camera_id = 1;
- break;
- case 2:
- // port 2
- i2c_info->slave_addr = 0x20;
- probe->camera_id = 2;
- break;
- }
-
- // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz
- //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE;
- i2c_info->i2c_freq_mode = I2C_FAST_MODE;
- i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO;
-
- probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD;
- probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
- probe->op_code = 3; // don't care?
- probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE;
- probe->reg_addr = 0x3000; //0x300a; //0x300b;
- probe->expected_data = 0x354; //0x7750; //0x885a;
- probe->data_mask = 0;
-
- //buf_desc[1].size = buf_desc[1].length = 148;
- buf_desc[1].size = buf_desc[1].length = 196;
- buf_desc[1].type = CAM_CMD_BUF_I2C;
- struct cam_cmd_power *power_settings = (struct cam_cmd_power *)alloc_w_mmu_hdl(video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
- memset(power_settings, 0, buf_desc[1].size);
- // 7750
- /*power->count = 2;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
- power->power_settings[0].power_seq_type = 2;
- power->power_settings[1].power_seq_type = 8;
- power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/
-
- // 885a
- struct cam_cmd_power *power = power_settings;
- power->count = 4;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
- power->power_settings[0].power_seq_type = 3; // clock??
- power->power_settings[1].power_seq_type = 1; // analog
- power->power_settings[2].power_seq_type = 2; // digital
- power->power_settings[3].power_seq_type = 8; // reset low
- power = power_set_wait(power, 5);
-
- // set clock
- power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
- power->power_settings[0].power_seq_type = 0;
- power->power_settings[0].config_val_low = 19200000; //Hz
- power = power_set_wait(power, 10);
-
- // 8,1 is this reset?
- power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
- power->power_settings[0].power_seq_type = 8;
- power->power_settings[0].config_val_low = 1;
- power = power_set_wait(power, 100);
-
- // probe happens here
-
- // disable clock
- power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
- power->power_settings[0].power_seq_type = 0;
- power->power_settings[0].config_val_low = 0;
- power = power_set_wait(power, 1);
-
- // reset high
- power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
- power->power_settings[0].power_seq_type = 8;
- power->power_settings[0].config_val_low = 1;
- power = power_set_wait(power, 1);
-
- // reset low
- power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
- power->power_settings[0].power_seq_type = 8;
- power->power_settings[0].config_val_low = 0;
- power = power_set_wait(power, 1);
-
- // 7750
- /*power->count = 1;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
- power->power_settings[0].power_seq_type = 2;
- power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/
-
- // 885a
- power->count = 3;
- power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN;
- power->power_settings[0].power_seq_type = 2;
- power->power_settings[1].power_seq_type = 1;
- power->power_settings[2].power_seq_type = 3;
-
- LOGD("probing the sensor");
- int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
- assert(ret == 0);
-
- munmap(i2c_info, buf_desc[0].size);
- release_fd(video0_fd, buf_desc[0].mem_handle);
- munmap(power_settings, buf_desc[1].size);
- release_fd(video0_fd, buf_desc[1].mem_handle);
- munmap(pkt, size);
- release_fd(video0_fd, cam_packet_handle);
-}
-
-void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
- uint32_t cam_packet_handle = 0;
- int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
- if (io_mem_handle != 0) {
- size += sizeof(struct cam_buf_io_cfg);
- }
- struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
- pkt->num_cmd_buf = 2;
- pkt->kmd_cmd_buf_index = 0;
-
- if (io_mem_handle != 0) {
- pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2;
- pkt->num_io_configs = 1;
- }
-
- if (io_mem_handle != 0) {
- pkt->header.op_code = 0xf000001;
- pkt->header.request_id = request_id;
- } else {
- pkt->header.op_code = 0xf000000;
- }
- pkt->header.size = size;
- struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
- struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
-
- // TODO: support MMU
- buf_desc[0].size = 65624;
- buf_desc[0].length = 0;
- buf_desc[0].type = CAM_CMD_BUF_DIRECT;
- buf_desc[0].meta_data = 3;
- buf_desc[0].mem_handle = buf0_mem_handle;
- buf_desc[0].offset = buf0_offset;
-
- buf_desc[1].size = 324;
- if (io_mem_handle != 0) {
- buf_desc[1].length = 228; // 0 works here too
- buf_desc[1].offset = 0x60;
- } else {
- buf_desc[1].length = 324;
- }
- buf_desc[1].type = CAM_CMD_BUF_GENERIC;
- buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
- uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
-
- // cam_isp_packet_generic_blob_handler
- uint32_t tmp[] = {
- // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG)
- 0x2000,
- 0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0
- // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks
- 0x3801,
- 0x1, 0x4, // Dual mode, 4 RDI wires
- 0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk?
- // offset 0x60
- // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth
- 0xe002,
- 0x1, 0x4, // 4 RDI
- 0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote
- 0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote
- 0x0, 0x0, 0x0, 0x0,
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
- 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
- memcpy(buf2, tmp, sizeof(tmp));
-
- if (io_mem_handle != 0) {
- io_cfg[0].mem_handle[0] = io_mem_handle;
- io_cfg[0].planes[0] = (struct cam_plane_cfg){
- .width = FRAME_WIDTH,
- .height = FRAME_HEIGHT,
- .plane_stride = FRAME_STRIDE,
- .slice_height = FRAME_HEIGHT,
- .meta_stride = 0x0,
- .meta_size = 0x0,
- .meta_offset = 0x0,
- .packer_config = 0x0,
- .mode_config = 0x0,
- .tile_config = 0x0,
- .h_init = 0x0,
- .v_init = 0x0,
- };
- io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;
- io_cfg[0].color_pattern = 0x5;
- io_cfg[0].bpp = 0xc;
- io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
- io_cfg[0].fence = fence;
- io_cfg[0].direction = CAM_BUF_OUTPUT;
- io_cfg[0].subsample_pattern = 0x1;
- io_cfg[0].framedrop_pattern = 0x1;
- }
-
- struct cam_config_dev_cmd config_dev_cmd = {};
- config_dev_cmd.session_handle = s->session_handle;
- config_dev_cmd.dev_handle = s->isp_dev_handle;
- config_dev_cmd.offset = 0;
- config_dev_cmd.packet_handle = cam_packet_handle;
-
- int ret = cam_control(s->multi_cam_state->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
- if (ret != 0) {
- printf("ISP CONFIG FAILED\n");
- }
-
- munmap(buf2, buf_desc[1].size);
- release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
- // release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
- munmap(pkt, size);
- release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
-}
-
-void enqueue_buffer(struct CameraState *s, int i, bool dp) {
- int ret;
- int request_id = s->request_ids[i];
-
- if (s->buf_handle[i]) {
- release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
- // wait
- struct cam_sync_wait sync_wait = {0};
- sync_wait.sync_obj = s->sync_objs[i];
- sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
- ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
- // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
-
- s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
- if (dp) s->buf.queue(i);
-
- // destroy old output fence
- struct cam_sync_info sync_destroy = {0};
- strcpy(sync_destroy.name, "NodeOutputPortFence");
- sync_destroy.sync_obj = s->sync_objs[i];
- ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
- // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
- }
-
- // do stuff
- struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
- req_mgr_sched_request.session_hdl = s->session_handle;
- req_mgr_sched_request.link_hdl = s->link_handle;
- req_mgr_sched_request.req_id = request_id;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
- // LOGD("sched req: %d %d", ret, request_id);
-
- // create output fence
- struct cam_sync_info sync_create = {0};
- strcpy(sync_create.name, "NodeOutputPortFence");
- ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
- // LOGD("fence req: %d %d", ret, sync_create.sync_obj);
- s->sync_objs[i] = sync_create.sync_obj;
-
- // configure ISP to put the image in place
- struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
- mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
- mem_mgr_map_cmd.num_hdl = 1;
- mem_mgr_map_cmd.flags = 1;
- mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
- // LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
- s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
-
- // poke sensor
- sensors_poke(s, request_id);
- // LOGD("Poked sensor");
-
- // push the buffer
- config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1));
-}
-
-void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
- for (int i=start;irequest_ids[(i - 1) % FRAME_BUF_COUNT] = i;
- enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
- }
-}
-
-// ******************* camera *******************
-
-static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
- LOGD("camera init %d", camera_num);
- s->multi_cam_state = multi_cam_state;
- assert(camera_id < std::size(cameras_supported));
- s->ci = cameras_supported[camera_id];
- assert(s->ci.frame_width != 0);
-
- s->camera_num = camera_num;
-
- s->dc_gain_enabled = false;
- s->analog_gain = 0x5;
- s->analog_gain_frac = sensor_analog_gains[s->analog_gain];
- s->exposure_time = 256;
- s->exposure_time_max = 1.2 * EXPOSURE_TIME_MAX / 2;
- s->exposure_time_min = 0.75 * EXPOSURE_TIME_MIN * 2;
- s->request_id_last = 0;
- s->skipped = true;
- s->ef_filtered = 1.0;
-
- s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
-}
-
-// TODO: refactor this to somewhere nicer, perhaps use in camera_qcom as well
-static int open_v4l_by_name_and_index(const char name[], int index, int flags) {
- char nbuf[0x100];
- int v4l_index = 0;
- int cnt_index = index;
- while (1) {
- snprintf(nbuf, sizeof(nbuf), "/sys/class/video4linux/v4l-subdev%d/name", v4l_index);
- FILE *f = fopen(nbuf, "rb");
- if (f == NULL) return -1;
- int len = fread(nbuf, 1, sizeof(nbuf), f);
- fclose(f);
-
- // name ends with '\n', remove it
- if (len < 1) return -1;
- nbuf[len-1] = '\0';
-
- if (strcmp(nbuf, name) == 0) {
- if (cnt_index == 0) {
- snprintf(nbuf, sizeof(nbuf), "/dev/v4l-subdev%d", v4l_index);
- LOGD("open %s for %s index %d", nbuf, name, index);
- return open(nbuf, flags);
- }
- cnt_index--;
- }
- v4l_index++;
- }
-}
-
-static void camera_open(CameraState *s) {
- int ret;
- s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num, O_RDWR | O_NONBLOCK);
- assert(s->sensor_fd >= 0);
- LOGD("opened sensor");
-
- s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num, O_RDWR | O_NONBLOCK);
- assert(s->csiphy_fd >= 0);
- LOGD("opened csiphy");
-
- // probe the sensor
- LOGD("-- Probing sensor %d", s->camera_num);
- sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
-
- memset(&s->req_mgr_session_info, 0, sizeof(s->req_mgr_session_info));
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
- LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl);
- s->session_handle = s->req_mgr_session_info.session_hdl;
- // access the sensor
- LOGD("-- Accessing sensor");
- static struct cam_acquire_dev_cmd acquire_dev_cmd = {0};
- acquire_dev_cmd.session_handle = s->session_handle;
- acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
- ret = cam_control(s->sensor_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
- LOGD("acquire sensor dev: %d", ret);
- s->sensor_dev_handle = acquire_dev_cmd.dev_handle;
-
- static struct cam_isp_resource isp_resource = {0};
-
- acquire_dev_cmd.session_handle = s->session_handle;
- acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
- acquire_dev_cmd.num_resources = 1;
- acquire_dev_cmd.resource_hdl = (uint64_t)&isp_resource;
-
- isp_resource.resource_id = CAM_ISP_RES_ID_PORT;
- isp_resource.length = sizeof(struct cam_isp_in_port_info) + sizeof(struct cam_isp_out_port_info)*(1-1);
- isp_resource.handle_type = CAM_HANDLE_USER_POINTER;
-
- struct cam_isp_in_port_info *in_port_info = (struct cam_isp_in_port_info *)malloc(isp_resource.length);
- isp_resource.res_hdl = (uint64_t)in_port_info;
-
- switch (s->camera_num) {
- case 0:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_0;
- break;
- case 1:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_1;
- break;
- case 2:
- in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_2;
- break;
- }
-
- in_port_info->lane_type = CAM_ISP_LANE_TYPE_DPHY;
- in_port_info->lane_num = 4;
- in_port_info->lane_cfg = 0x3210;
-
- in_port_info->vc = 0x0;
- //in_port_info->dt = 0x2C; //CSI_RAW12
- //in_port_info->format = CAM_FORMAT_MIPI_RAW_12;
-
- in_port_info->dt = 0x2B; //CSI_RAW10
- in_port_info->format = CAM_FORMAT_MIPI_RAW_10;
-
- in_port_info->test_pattern = 0x2; // 0x3?
- in_port_info->usage_type = 0x0;
-
- in_port_info->left_start = 0x0;
- in_port_info->left_stop = FRAME_WIDTH - 1;
- in_port_info->left_width = FRAME_WIDTH;
-
- in_port_info->right_start = 0x0;
- in_port_info->right_stop = FRAME_WIDTH - 1;
- in_port_info->right_width = FRAME_WIDTH;
-
- in_port_info->line_start = 0x0;
- in_port_info->line_stop = FRAME_HEIGHT - 1;
- in_port_info->height = FRAME_HEIGHT;
-
- in_port_info->pixel_clk = 0x0;
- in_port_info->batch_size = 0x0;
- in_port_info->dsp_mode = 0x0;
- in_port_info->hbi_cnt = 0x0;
- in_port_info->custom_csid = 0x0;
-
- in_port_info->num_out_res = 0x1;
- in_port_info->data[0] = (struct cam_isp_out_port_info){
- .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
- //.format = CAM_FORMAT_MIPI_RAW_12,
- .format = CAM_FORMAT_MIPI_RAW_10,
- .width = FRAME_WIDTH,
- .height = FRAME_HEIGHT,
- .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
- };
-
- ret = cam_control(s->multi_cam_state->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
- LOGD("acquire isp dev: %d", ret);
- free(in_port_info);
- s->isp_dev_handle = acquire_dev_cmd.dev_handle;
-
- static struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {0};
- csiphy_acquire_dev_info.combo_mode = 0;
-
- acquire_dev_cmd.session_handle = s->session_handle;
- acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
- acquire_dev_cmd.num_resources = 1;
- acquire_dev_cmd.resource_hdl = (uint64_t)&csiphy_acquire_dev_info;
-
- ret = cam_control(s->csiphy_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
-
- LOGD("acquire csiphy dev: %d", ret);
- s->csiphy_dev_handle = acquire_dev_cmd.dev_handle;
-
- // acquires done
-
- // config ISP
- alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
- config_isp(s, 0, 0, 1, s->buf0_handle, 0);
-
- LOG("-- Configuring sensor");
- sensors_i2c(s, init_array_ar0231, sizeof(init_array_ar0231)/sizeof(struct i2c_random_wr_payload),
- CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
- //sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
- //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
- //sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload),
- //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
-
- // config csiphy
- LOG("-- Config CSI PHY");
- {
- uint32_t cam_packet_handle = 0;
- int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
- struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
- pkt->num_cmd_buf = 1;
- pkt->kmd_cmd_buf_index = -1;
- pkt->header.size = size;
- struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
-
- buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
- buf_desc[0].type = CAM_CMD_BUF_GENERIC;
-
- struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
- csiphy_info->lane_mask = 0x1f;
- csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
- csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
- csiphy_info->combo_mode = 0x0;
- csiphy_info->lane_cnt = 0x4;
- csiphy_info->secure_mode = 0x0;
- csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
- csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py
-
- static struct cam_config_dev_cmd config_dev_cmd = {};
- config_dev_cmd.session_handle = s->session_handle;
- config_dev_cmd.dev_handle = s->csiphy_dev_handle;
- config_dev_cmd.offset = 0;
- config_dev_cmd.packet_handle = cam_packet_handle;
-
- int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
- assert(ret == 0);
-
- release(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
- release(s->multi_cam_state->video0_fd, cam_packet_handle);
- }
-
- // link devices
- LOG("-- Link devices");
- static struct cam_req_mgr_link_info req_mgr_link_info = {0};
- req_mgr_link_info.session_hdl = s->session_handle;
- req_mgr_link_info.num_devices = 2;
- req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
- req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
- LOGD("link: %d", ret);
- s->link_handle = req_mgr_link_info.link_hdl;
-
- static struct cam_req_mgr_link_control req_mgr_link_control = {0};
- req_mgr_link_control.ops = 0;
- req_mgr_link_control.session_hdl = s->session_handle;
- req_mgr_link_control.num_links = 1;
- req_mgr_link_control.link_hdls[0] = s->link_handle;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
- LOGD("link control: %d", ret);
-
- LOGD("start csiphy: %d", ret);
- ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
- LOGD("start isp: %d", ret);
- ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
- LOGD("start sensor: %d", ret);
- ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
-
- enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0);
-}
-
-void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
- camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right
- printf("road camera initted \n");
- camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
- VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE);
- printf("wide road camera initted \n");
- camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
- printf("driver camera initted \n");
-
- s->sm = new SubMaster({"driverState"});
- s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
-}
-
-void cameras_open(MultiCameraState *s) {
- int ret;
-
- LOG("-- Opening devices");
- // video0 is req_mgr, the target of many ioctls
- s->video0_fd = open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK);
- assert(s->video0_fd >= 0);
- LOGD("opened video0");
-
- // video1 is cam_sync, the target of some ioctls
- s->video1_fd = open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK);
- assert(s->video1_fd >= 0);
- LOGD("opened video1");
-
- // looks like there's only one of these
- s->isp_fd = open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK);
- assert(s->isp_fd >= 0);
- LOGD("opened isp");
-
- // query icp for MMU handles
- LOG("-- Query ICP for MMU handles");
- static struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0};
- static struct cam_query_cap_cmd query_cap_cmd = {0};
- query_cap_cmd.handle_type = 1;
- query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd;
- query_cap_cmd.size = sizeof(isp_query_cap_cmd);
- ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
- assert(ret == 0);
- LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
- LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
- s->device_iommu = isp_query_cap_cmd.device_iommu.non_secure;
- s->cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure;
-
- // subscribe
- LOG("-- Subscribing");
- static struct v4l2_event_subscription sub = {0};
- sub.type = 0x8000000;
- sub.id = 2; // should use boot time for sof
- ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
- printf("req mgr subscribe: %d\n", ret);
-
- camera_open(&s->road_cam);
- printf("road camera opened \n");
- camera_open(&s->wide_road_cam);
- printf("wide road camera opened \n");
- camera_open(&s->driver_cam);
- printf("driver camera opened \n");
-}
-
-static void camera_close(CameraState *s) {
- int ret;
-
- // stop devices
- LOG("-- Stop devices");
- // ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
- // LOGD("stop sensor: %d", ret);
- ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
- LOGD("stop isp: %d", ret);
- ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
- LOGD("stop csiphy: %d", ret);
- // link control stop
- LOG("-- Stop link control");
- static struct cam_req_mgr_link_control req_mgr_link_control = {0};
- req_mgr_link_control.ops = 1;
- req_mgr_link_control.session_hdl = s->session_handle;
- req_mgr_link_control.num_links = 1;
- req_mgr_link_control.link_hdls[0] = s->link_handle;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
- LOGD("link control stop: %d", ret);
-
- // unlink
- LOG("-- Unlink");
- static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
- req_mgr_unlink_info.session_hdl = s->session_handle;
- req_mgr_unlink_info.link_hdl = s->link_handle;
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
- LOGD("unlink: %d", ret);
-
- // release devices
- LOGD("-- Release devices");
- ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
- LOGD("release sensor: %d", ret);
- ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
- LOGD("release isp: %d", ret);
- ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
- LOGD("release csiphy: %d", ret);
-
- ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
- LOGD("destroyed session: %d", ret);
-}
-
-void cameras_close(MultiCameraState *s) {
- camera_close(&s->road_cam);
- camera_close(&s->wide_road_cam);
- camera_close(&s->driver_cam);
-
- delete s->sm;
- delete s->pm;
-}
-
-// ******************* just a helper *******************
-
-void handle_camera_event(CameraState *s, void *evdat) {
- struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
-
- uint64_t timestamp = event_data->u.frame_msg.timestamp;
- int main_id = event_data->u.frame_msg.frame_id;
- int real_id = event_data->u.frame_msg.request_id;
-
- if (real_id != 0) { // next ready
- if (real_id == 1) {s->idx_offset = main_id;}
- int buf_idx = (real_id - 1) % FRAME_BUF_COUNT;
-
- // check for skipped frames
- if (main_id > s->frame_id_last + 1 && !s->skipped) {
- // realign
- clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
- enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
- s->skipped = true;
- } else if (main_id == s->frame_id_last + 1) {
- s->skipped = false;
- }
-
- // check for dropped requests
- if (real_id > s->request_id_last + 1) {
- enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
- }
-
- // metas
- s->frame_id_last = main_id;
- s->request_id_last = real_id;
-
- auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
- meta_data.frame_id = main_id - s->idx_offset;
- meta_data.timestamp_sof = timestamp;
- s->exp_lock.lock();
- meta_data.global_gain = s->analog_gain + (100*s->dc_gain_enabled);
- meta_data.gain_frac = s->analog_gain_frac;
- meta_data.integ_lines = s->exposure_time;
- s->exp_lock.unlock();
-
- // dispatch
- enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
- } else { // not ready
- // reset after half second of no response
- if (main_id > s->frame_id_last + 10) {
- clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
- enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
- s->frame_id_last = main_id;
- s->skipped = true;
- }
- }
-}
-
-// ******************* exposure control helpers *******************
-
-void set_exposure_time_bounds(CameraState *s) {
- switch (s->analog_gain) {
- case 0: {
- s->exposure_time_min = EXPOSURE_TIME_MIN;
- s->exposure_time_max = EXPOSURE_TIME_MAX; // EXPOSURE_TIME_MIN * 4;
- break;
- }
- case ANALOG_GAIN_MAX_IDX - 1: {
- s->exposure_time_min = EXPOSURE_TIME_MIN; // EXPOSURE_TIME_MAX / 4;
- s->exposure_time_max = EXPOSURE_TIME_MAX;
- break;
- }
- default: {
- // finetune margins on both ends
- float k_up = sensor_analog_gains[s->analog_gain+1] / sensor_analog_gains[s->analog_gain];
- float k_down = sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain];
- s->exposure_time_min = k_down * EXPOSURE_TIME_MIN * 2;
- s->exposure_time_max = k_up * EXPOSURE_TIME_MAX / 2;
- }
- }
-}
-
-void switch_conversion_gain(CameraState *s) {
- if (!s->dc_gain_enabled) {
- s->dc_gain_enabled = true;
- s->analog_gain -= 4;
- } else {
- s->dc_gain_enabled = false;
- s->analog_gain += 4;
- }
-}
-
-static void set_camera_exposure(CameraState *s, float grey_frac) {
- // TODO: get stats from sensor?
- float target_grey = 0.4 - ((float)(s->analog_gain + 4*s->dc_gain_enabled) / 48.0f);
- float exposure_factor = 1 + 30 * pow((target_grey - grey_frac), 3);
- exposure_factor = std::max(exposure_factor, 0.56f);
-
- if (s->camera_num != 1) {
- s->ef_filtered = (1 - EF_LOWPASS_K) * s->ef_filtered + EF_LOWPASS_K * exposure_factor;
- exposure_factor = s->ef_filtered;
- }
-
- s->exp_lock.lock();
- // always prioritize exposure time adjust
- s->exposure_time *= exposure_factor;
-
- // switch gain if max/min exposure time is reached
- // or always switch down to a lower gain when possible
- bool kd = false;
- if (s->analog_gain > 0) {
- kd = 1.1 * s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain]) < EXPOSURE_TIME_MAX / 2;
- }
-
- if (s->exposure_time > s->exposure_time_max) {
- if (s->analog_gain < ANALOG_GAIN_MAX_IDX - 1) {
- s->exposure_time = EXPOSURE_TIME_MAX / 2;
- s->analog_gain += 1;
- if (!s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] >= 4.0) { // switch to HCG
- switch_conversion_gain(s);
- }
- set_exposure_time_bounds(s);
- } else {
- s->exposure_time = s->exposure_time_max;
- }
- } else if (s->exposure_time < s->exposure_time_min || kd) {
- if (s->analog_gain > 0) {
- s->exposure_time = std::max(EXPOSURE_TIME_MIN * 2, (int)(s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain])));
- s->analog_gain -= 1;
- if (s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] <= 1.25) { // switch back to LCG
- switch_conversion_gain(s);
- }
- set_exposure_time_bounds(s);
- } else {
- s->exposure_time = s->exposure_time_min;
- }
- }
-
- // set up config
- uint16_t AG = s->analog_gain + 4;
- AG = 0xFF00 + AG * 16 + AG;
- s->analog_gain_frac = sensor_analog_gains[s->analog_gain];
-
- s->exp_lock.unlock();
- // printf("cam %d, min %d, max %d \n", s->camera_num, s->exposure_time_min, s->exposure_time_max);
- // printf("cam %d, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, AG, s->exposure_time, s->dc_gain_enabled);
-
- struct i2c_random_wr_payload exp_reg_array[] = {
- {0x3366, AG}, // analog gain
- {0x3362, (uint16_t)(s->dc_gain_enabled?0x1:0x0)}, // DC_GAIN
- {0x305A, 0x00F8}, // red gain
- {0x3058, 0x0122}, // blue gain
- {0x3056, 0x009A}, // g1 gain
- {0x305C, 0x009A}, // g2 gain
- {0x3012, (uint16_t)s->exposure_time}, // integ time
- };
- //{0x301A, 0x091C}}; // reset
- sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
- CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-}
-
-void camera_autoexposure(CameraState *s, float grey_frac) {
- CameraExpInfo tmp = cam_exp[s->camera_num].load();
- tmp.op_id++;
- tmp.grey_frac = grey_frac;
- cam_exp[s->camera_num].store(tmp);
-}
-
-static void ae_thread(MultiCameraState *s) {
- CameraState *c_handles[3] = {&s->wide_road_cam, &s->road_cam, &s->driver_cam};
-
- int op_id_last[3] = {0};
- CameraExpInfo cam_op[3];
-
- set_thread_name("camera_settings");
-
- while(!do_exit) {
- for (int i=0;i<3;i++) {
- cam_op[i] = cam_exp[i].load();
- if (cam_op[i].op_id != op_id_last[i]) {
- set_camera_exposure(c_handles[i], cam_op[i].grey_frac);
- op_id_last[i] = cam_op[i].op_id;
- }
- }
-
- util::sleep_for(50);
- }
-}
-
-void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
- common_process_driver_camera(s->sm, s->pm, c, cnt);
-}
-
-// called by processing_thread
-void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
- const CameraBuf *b = &c->buf;
-
- MessageBuilder msg;
- auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState();
- fill_frame_data(framed, b->cur_frame_data);
- if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) {
- framed.setImage(get_frame_image(b));
- }
- if (c == &s->road_cam) {
- framed.setTransform(b->yuv_transform.v);
- }
- s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
-
- if (cnt % 3 == 0) {
- const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
- const int skip = 2;
- camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip, (int)c->analog_gain, true, true));
- }
-}
-
-void cameras_run(MultiCameraState *s) {
- LOG("-- Starting threads");
- std::vector threads;
- threads.push_back(std::thread(ae_thread, s));
- threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
- threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
- threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
-
- // start devices
- LOG("-- Starting devices");
- int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
- sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
- sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
- sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-
- // poll events
- LOG("-- Dequeueing Video events");
- while (!do_exit) {
- struct pollfd fds[1] = {{0}};
-
- fds[0].fd = s->video0_fd;
- fds[0].events = POLLPRI;
-
- int ret = poll(fds, std::size(fds), 1000);
- if (ret < 0) {
- if (errno == EINTR || errno == EAGAIN) continue;
- LOGE("poll failed (%d - %d)", ret, errno);
- break;
- }
-
- if (!fds[0].revents) continue;
-
- struct v4l2_event ev = {0};
- ret = ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev);
- if (ev.type == 0x8000000) {
- struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
- // LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
- // printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
-
- if (event_data->session_hdl == s->road_cam.req_mgr_session_info.session_hdl) {
- handle_camera_event(&s->road_cam, event_data);
- } else if (event_data->session_hdl == s->wide_road_cam.req_mgr_session_info.session_hdl) {
- handle_camera_event(&s->wide_road_cam, event_data);
- } else if (event_data->session_hdl == s->driver_cam.req_mgr_session_info.session_hdl) {
- handle_camera_event(&s->driver_cam, event_data);
- } else {
- printf("Unknown vidioc event source\n");
- assert(false);
- }
- }
- }
-
- LOG(" ************** STOPPING **************");
-
- for (auto &t : threads) t.join();
-
- cameras_close(s);
-}
diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h
deleted file mode 100644
index 851dab9f4..000000000
--- a/selfdrive/camerad/cameras/camera_qcom2.h
+++ /dev/null
@@ -1,79 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-
-#include "selfdrive/camerad/cameras/camera_common.h"
-#include "selfdrive/common/util.h"
-
-#define FRAME_BUF_COUNT 4
-
-#define ANALOG_GAIN_MAX_IDX 10 // 0xF is bypass
-#define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss
-#define EXPOSURE_TIME_MAX 1904 // with HDR, slowest ss
-
-#define EF_LOWPASS_K 0.35
-
-#define DEBAYER_LOCAL_WORKSIZE 16
-
-typedef struct CameraState {
- MultiCameraState *multi_cam_state;
- CameraInfo ci;
-
- std::mutex exp_lock;
- float analog_gain_frac;
- uint16_t analog_gain;
- bool dc_gain_enabled;
- int exposure_time;
- int exposure_time_min;
- int exposure_time_max;
- float ef_filtered;
-
- unique_fd sensor_fd;
- unique_fd csiphy_fd;
-
- int camera_num;
-
- uint32_t session_handle;
-
- uint32_t sensor_dev_handle;
- uint32_t isp_dev_handle;
- uint32_t csiphy_dev_handle;
-
- int32_t link_handle;
-
- int buf0_handle;
- int buf_handle[FRAME_BUF_COUNT];
- int sync_objs[FRAME_BUF_COUNT];
- int request_ids[FRAME_BUF_COUNT];
- int request_id_last;
- int frame_id_last;
- int idx_offset;
- bool skipped;
-
- struct cam_req_mgr_session_info req_mgr_session_info;
-
- CameraBuf buf;
-} CameraState;
-
-typedef struct MultiCameraState {
- int device;
-
- unique_fd video0_fd;
- unique_fd video1_fd;
- unique_fd isp_fd;
- int device_iommu;
- int cdm_iommu;
-
-
- CameraState road_cam;
- CameraState wide_road_cam;
- CameraState driver_cam;
-
- pthread_mutex_t isp_lock;
-
- SubMaster *sm;
- PubMaster *pm;
-} MultiCameraState;
diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h
index 5898bd4cc..38a05820f 100644
--- a/selfdrive/camerad/cameras/sensor2_i2c.h
+++ b/selfdrive/camerad/cameras/sensor2_i2c.h
@@ -56,7 +56,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x337A, 0x0C80}, // DBLC_SCALE0
{0x3370, 0x03B1}, // DBLC
{0x3044, 0x0400}, // DARK_CONTROL
- {0x31E0, 0x0001}, // PDC
+
+ // Enable dead pixel correction using
+ // the 1D line correction scheme
+ {0x31E0, 0x0003},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc
index 5c2110131..ad6452c6a 100644
--- a/selfdrive/camerad/imgproc/utils.cc
+++ b/selfdrive/camerad/imgproc/utils.cc
@@ -1,11 +1,10 @@
#include "selfdrive/camerad/imgproc/utils.h"
-#include
-#include
-#include
-
#include
+#include
+#include
#include
+#include
const int16_t lapl_conv_krnl[9] = {0, 1, 0,
1, -4, 1,
diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h
index d7d518c0f..72baa5d53 100644
--- a/selfdrive/camerad/imgproc/utils.h
+++ b/selfdrive/camerad/imgproc/utils.h
@@ -1,8 +1,7 @@
#pragma once
-#include
-#include
-
+#include
+#include
#include
#include "selfdrive/common/clutil.h"
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index 9636fd27b..b4cf162d2 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -1,9 +1,9 @@
-#include
#include
-#include
#include
#include
+#include
+#include
#include
#include "libyuv.h"
diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.cc b/selfdrive/camerad/transforms/rgb_to_yuv.cc
index 1fdeb593d..63e032e2d 100644
--- a/selfdrive/camerad/transforms/rgb_to_yuv.cc
+++ b/selfdrive/camerad/transforms/rgb_to_yuv.cc
@@ -1,6 +1,7 @@
-#include "rgb_to_yuv.h"
+#include "selfdrive/camerad/transforms/rgb_to_yuv.h"
-#include
+#include
+#include
Rgb2Yuv::Rgb2Yuv(cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) {
assert(width % 2 == 0 && height % 2 == 0);
diff --git a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
index 04b6afd09..74db2bc1c 100644
--- a/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
+++ b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc
@@ -1,11 +1,11 @@
#include
#include
-#include
#include
-#include
#include
#include
+#include
+#include
#include
#include
#include
@@ -145,8 +145,8 @@ int main(int argc, char** argv) {
int counter = 0;
srand (time(NULL));
- for (int i = 0; i < 100; i++){
- for (int i = 0; i < width * height * 3; i++){
+ for (int i = 0; i < 100; i++) {
+ for (int i = 0; i < width * height * 3; i++) {
rgb_frame[i] = (uint8_t)rand();
}
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index ba214410d..c0c4f4bf0 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -170,7 +170,8 @@ def fingerprint(logcan, sendcan):
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
- cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match)
+ cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint,
+ source=source, fuzzy=not exact_match, fw_count=len(car_fw))
return car_fingerprint, finger, vin, car_fw, source, exact_match
diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py
index 63d1fbe0b..c72f15529 100644
--- a/selfdrive/car/chrysler/chryslercan.py
+++ b/selfdrive/car/chrysler/chryslercan.py
@@ -8,7 +8,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model):
# LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
- if hud_alert == VisualAlert.steerRequired:
+ if hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]:
msg = b'\x00\x00\x00\x03\x00\x00\x00\x00'
return make_can_msg(0x2a6, msg, 0)
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index f37ae953a..263e31706 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -49,8 +49,6 @@ class CarInterface(CarInterfaceBase):
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
- ret.enableCamera = True
-
ret.enableBsm = 720 in fingerprint[0]
return ret
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 8f842083e..04fd9ec08 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -20,7 +20,7 @@ class CAR:
JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk
JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk
-# Unique can messages:
+# Unique CAN messages:
# Only the hybrids have 270: 8
# Only the gas have 55: 8, 416: 7
# For 564, All 2017 have length 4, whereas 2018-19 have length 8.
@@ -32,46 +32,52 @@ class CAR:
# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8.
FINGERPRINTS = {
- CAR.PACIFICA_2017_HYBRID: [
- {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8},
- ],
- CAR.PACIFICA_2018: [
- {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
- {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
- ],
- CAR.PACIFICA_2020: [{
- 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8
+ CAR.PACIFICA_2017_HYBRID: [{
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8
}],
- CAR.PACIFICA_2018_HYBRID: [
- {68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},
+ CAR.PACIFICA_2018: [{
+ 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
+ },
+ {
+ 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
+ }],
+ CAR.PACIFICA_2020: [{
+ 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8
+ }],
+ CAR.PACIFICA_2018_HYBRID: [{
+ 68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
+ },
# based on 9ae7821dc4e92455|2019-07-01--16-42-55
- {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8},
- ],
- CAR.PACIFICA_2019_HYBRID: [
- {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
+ {
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
+ }],
+ CAR.PACIFICA_2019_HYBRID: [{
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8
+ },
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
- {
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
- },
+ {
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
+ },
# Based on 3c7ce223e3571b54|2019-05-11--20-16-14
- {
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8
- },
+ {
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8
+ },
# Based on "8190c7275a24557b|2020-02-24--09-57-23"
- {
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
- }
- ],
- CAR.JEEP_CHEROKEE: [
+ {
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8
+ }],
+ CAR.JEEP_CHEROKEE: [{
# JEEP GRAND CHEROKEE V6 2018
- {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
+ 55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8
+ },
# Jeep Grand Cherokee 2017 Trailhawk
- {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
+ {
+ 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
],
- CAR.JEEP_CHEROKEE_2019: [
+ CAR.JEEP_CHEROKEE_2019: [{
# Jeep Grand Cherokee 2019, including most 2020 models
- {55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8},
- ],
+ 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
+ }],
}
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 0949ffe22..06c5f2579 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -4,6 +4,7 @@ from selfdrive.car import make_can_msg
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button
from opendbc.can.packer import CANPacker
+VisualAlert = car.CarControl.HUDControl.VisualAlert
MAX_STEER_DELTA = 1
TOGGLE_DEBUG = False
@@ -11,7 +12,6 @@ TOGGLE_DEBUG = False
class CarController():
def __init__(self, dbc_name, CP, VM):
self.packer = CANPacker(dbc_name)
- self.enable_camera = CP.enableCamera
self.enabled_last = False
self.main_on_last = False
self.vehicle_model = VM
@@ -22,67 +22,65 @@ class CarController():
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
- steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
+ steer_alert = visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
apply_steer = actuators.steer
- if self.enable_camera:
+ if pcm_cancel:
+ #print "CANCELING!!!!"
+ can_sends.append(spam_cancel_button(self.packer))
- if pcm_cancel:
- #print "CANCELING!!!!"
- can_sends.append(spam_cancel_button(self.packer))
+ if (frame % 3) == 0:
- if (frame % 3) == 0:
+ curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
- curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
+ # The use of the toggle below is handy for trying out the various LKAS modes
+ if TOGGLE_DEBUG:
+ self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last)
+ self.lkas_action &= 0xf
+ else:
+ self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
- # The use of the toggle below is handy for trying out the various LKAS modes
- if TOGGLE_DEBUG:
- self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last)
- self.lkas_action &= 0xf
- else:
- self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
+ can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
+ CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action))
+ self.generic_toggle_last = CS.out.genericToggle
- can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
- CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action))
- self.generic_toggle_last = CS.out.genericToggle
+ if (frame % 100) == 0:
- if (frame % 100) == 0:
+ can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
+ #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
- can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
- #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
+ if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
+ (self.steer_alert_last != steer_alert):
+ can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert))
- if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
- (self.steer_alert_last != steer_alert):
- can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert))
+ if (frame % 200) == 0:
+ can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
- if (frame % 200) == 0:
- can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
+ if (frame % 10) == 0:
- if (frame % 10) == 0:
+ can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
+ can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
- can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
+ can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
+ can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
+ can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
+ can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
- can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
- can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
- can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
+ can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
+ can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
+ can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
+ can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
+ can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
+ can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
- can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
- can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
- can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
- can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
+ static_msgs = range(1653, 1658)
+ for addr in static_msgs:
+ cnt = (frame % 10) + 1
+ can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
- static_msgs = range(1653, 1658)
- for addr in static_msgs:
- cnt = (frame % 10) + 1
- can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
-
- self.enabled_last = enabled
- self.main_on_last = CS.out.cruiseState.available
- self.steer_alert_last = steer_alert
+ self.enabled_last = enabled
+ self.main_on_last = CS.out.cruiseState.available
+ self.steer_alert_last = steer_alert
return can_sends
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index aec368cd0..f5923515e 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
from cereal import car
-from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.car.ford.values import MAX_ANGLE
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
@@ -43,9 +42,6 @@ class CarInterface(CarInterfaceBase):
ret.steerControlType = car.CarParams.SteerControlType.angle
- ret.enableCamera = True
- cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
-
return ret
# returns a car.CarState
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index 116a562ac..71490b1cb 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -100,7 +100,7 @@ class CarController():
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
- steer_alert = hud_alert == VisualAlert.steerRequired
+ steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 7c28406cb..85578cd98 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "gm"
ret.safetyModel = car.CarParams.SafetyModel.gm
- ret.enableCruise = False # stock cruise control is kept off
+ ret.pcmCruise = False # stock cruise control is kept off
# GM port is a community feature
# TODO: make a port that uses a car harness and it only intercepts the camera
@@ -29,8 +29,7 @@ class CarInterface(CarInterfaceBase):
# Presence of a camera on the object bus is ok.
# Have to go to read_only if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
- ret.enableCamera = True
- ret.openpilotLongitudinalControl = ret.enableCamera
+ ret.openpilotLongitudinalControl = True
tire_stiffness_factor = 0.444 # not optimized yet
# Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below.
@@ -106,7 +105,6 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.36]
- ret.stoppingControl = True
ret.startAccel = 0.8
ret.steerLimitTimer = 0.4
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index 9f8d9fa75..6f6b0d722 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -89,6 +89,10 @@ FINGERPRINTS = {
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8,
}],
CAR.ACADIA: [
+ # Acadia Denali w/ACC 2018
+ {
+ 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7
+ },
# Acadia Denali w/ /ACC 2018
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 53304583b..3f43fc581 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -10,6 +10,7 @@ from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
+
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
@@ -62,7 +63,7 @@ def process_hud_alert(hud_alert):
# priority is: FCW, steer required, all others
if hud_alert == VisualAlert.fcw:
fcw_display = VISUAL_HUD[hud_alert.raw]
- elif hud_alert == VisualAlert.steerRequired:
+ elif hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]:
steer_required = VISUAL_HUD[hud_alert.raw]
else:
acc_alert = VISUAL_HUD[hud_alert.raw]
@@ -103,7 +104,7 @@ class CarController():
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
- pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.enableCruise
+ pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
@@ -137,6 +138,11 @@ class CarController():
# Send CAN commands.
can_sends = []
+ # tester present - w/ no response (keeps radar disabled)
+ if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
+ if (frame % 10) == 0:
+ can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
+
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
@@ -163,7 +169,19 @@ class CarController():
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
- pass # TODO: implement
+ accel = actuators.gas - actuators.brake
+
+ # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
+ stopping = accel < 0 and CS.out.vEgo < 0.3
+ starting = accel > 0 and CS.out.vEgo < 0.3
+
+ # Prevent rolling backwards
+ accel = -1.0 if stopping else accel
+
+ apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V)
+ apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
+ can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint))
+
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index d081fef0d..3067c5e29 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -92,29 +92,27 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
signals += [
("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_FEEDBACK", 0),
- ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
("EPB_STATE", "EPB_STATUS", 0),
- ("CRUISE_SPEED", "ACC_HUD", 0),
- ("ACCEL_COMMAND", "ACC_CONTROL", 0),
- ("AEB_STATUS", "ACC_CONTROL", 0),
]
checks += [
- ("ACC_HUD", 10),
("EPB_STATUS", 50),
("GAS_PEDAL_2", 100),
- ("ACC_CONTROL", 50),
]
- if CP.openpilotLongitudinalControl:
- signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
- ("BRAKE_ERROR_2", "STANDSTILL", 1)]
- checks += [("STANDSTILL", 50)]
- else:
- # Nidec signals.
- signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
- ("BRAKE_ERROR_2", "STANDSTILL", 1),
- ("CRUISE_SPEED_PCM", "CRUISE", 0),
+
+ if not CP.openpilotLongitudinalControl:
+ signals += [
+ ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
+ ("CRUISE_SPEED", "ACC_HUD", 0),
+ ("ACCEL_COMMAND", "ACC_CONTROL", 0),
+ ("AEB_STATUS", "ACC_CONTROL", 0),
+ ]
+ checks += [
+ ("ACC_HUD", 10),
+ ("ACC_CONTROL", 50),
+ ]
+ else: # Nidec signals
+ signals += [("CRUISE_SPEED_PCM", "CRUISE", 0),
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
- checks += [("STANDSTILL", 50)]
if CP.carFingerprint == CAR.ODYSSEY_CHN:
checks += [("CRUISE_PARAMS", 10)]
@@ -192,6 +190,13 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
+ if CP.openpilotLongitudinalControl:
+ signals += [
+ ("BRAKE_ERROR_1", "STANDSTILL", 1),
+ ("BRAKE_ERROR_2", "STANDSTILL", 1)
+ ]
+ checks += [("STANDSTILL", 50)]
+
return signals, checks
@@ -211,7 +216,6 @@ class CarState(CarStateBase):
self.brake_switch_prev_ts = 0
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
- self.cruise_mode = 0
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
@@ -271,8 +275,8 @@ class CarState(CarStateBase):
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
- ret.leftBlinker = cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"] != 0
- ret.rightBlinker = cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"] != 0
+ ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
+ 250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
@@ -310,12 +314,13 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
if self.CP.carFingerprint in HONDA_BOSCH:
- self.cruise_mode = cp.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"]
- ret.cruiseState.standstill = cp.vl["ACC_HUD"]["CRUISE_SPEED"] == 252.
- ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
- # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
- ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS
- self.v_cruise_pcm_prev = ret.cruiseState.speed
+ if not self.CP.openpilotLongitudinalControl:
+ ret.cruiseState.nonAdaptive = cp.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0
+ ret.cruiseState.standstill = cp.vl["ACC_HUD"]["CRUISE_SPEED"] == 252.
+
+ # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
+ ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS
+ self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo)
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
@@ -336,7 +341,6 @@ class CarState(CarStateBase):
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
ret.cruiseState.available = bool(main_on)
- ret.cruiseState.nonAdaptive = self.cruise_mode != 0
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
@@ -347,7 +351,7 @@ class CarState(CarStateBase):
self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"] if self.CP.carFingerprint in (CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
- ret.stockAeb = bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
+ ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index ef2564702..883d4fe94 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -1,3 +1,5 @@
+from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import HONDA_BOSCH
@@ -7,6 +9,13 @@ from selfdrive.car.honda.values import HONDA_BOSCH
# 2 = ACC-CAN - camera side
# 3 = F-CAN A - OBDII port
+RADAR_ADDR = 0x18DAB0F1
+EXT_DIAG_REQUEST = b'\x10\x03'
+EXT_DIAG_RESPONSE = b'\x50\x03'
+COM_CONT_REQUEST = b'\x28\x83\x03'
+COM_CONT_RESPONSE = b''
+
+
def get_pt_bus(car_fingerprint):
return 1 if car_fingerprint in HONDA_BOSCH else 0
@@ -19,6 +28,29 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
return 0
+def disable_radar(logcan, sendcan, bus=1, timeout=0.1, debug=False):
+ """Silence the radar by disabling sending and receiving messages using UDS 0x28.
+ The radar will stay silent as long as openpilot keeps sending Tester Present.
+ Openpilot will emulate the radar. WARNING: THIS DISABLES AEB!"""
+ cloudlog.warning(f"radar disable {hex(RADAR_ADDR)} ...")
+
+ try:
+ query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+
+ for _, _ in query.get_data(timeout).items():
+ cloudlog.warning("radar communication control disable tx/rx ...")
+
+ query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [COM_CONT_REQUEST], [COM_CONT_RESPONSE], debug=debug)
+ query.get_data(0)
+
+ cloudlog.warning("radar disabled")
+ return
+
+ except Exception:
+ cloudlog.exception("radar disable exception")
+ cloudlog.warning("radar disable failed")
+
+
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 65d61a70d..15446623c 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -1,22 +1,26 @@
#!/usr/bin/env python3
import numpy as np
from cereal import car
+from panda import Panda
from common.numpy_fast import clip, interp
-from selfdrive.swaglog import cloudlog
+from common.params import Params
from selfdrive.config import Conversions as CV
-from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.hondacan import disable_radar
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
-from selfdrive.controls.lib.longitudinal_planner import _A_CRUISE_MAX_V_FOLLOWING
from selfdrive.car.interfaces import CarInterfaceBase
-A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
TransmissionType = car.CarParams.TransmissionType
-def compute_gb_honda(accel, speed):
+def compute_gb_honda_bosch(accel, speed):
+ return float(accel) / 3.5
+
+
+def compute_gb_honda_nidec(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
creep_brake_value = 0.15
@@ -76,8 +80,10 @@ class CarInterface(CarInterfaceBase):
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
self.compute_gb = get_compute_gb_acura()
+ elif self.CS.CP.carFingerprint in HONDA_BOSCH:
+ self.compute_gb = compute_gb_honda_bosch
else:
- self.compute_gb = compute_gb_honda
+ self.compute_gb = compute_gb_honda_nidec
@staticmethod
def compute_gb(accel, speed): # pylint: disable=method-hidden
@@ -113,7 +119,7 @@ class CarInterface(CarInterfaceBase):
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this help in quicker restart
- return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
+ return float(max(max_accel, a_target / CarControllerParams.ACCEL_MAX)) * min(speedLimiter, accelLimiter)
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
@@ -122,14 +128,21 @@ class CarInterface(CarInterfaceBase):
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness
- ret.enableCamera = True
ret.radarOffCan = True
- ret.openpilotLongitudinalControl = False
+
+ # Disable the radar and let openpilot control longitudinal
+ # WARNING: THIS DISABLES AEB!
+ ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar")
+
+ ret.pcmCruise = not ret.openpilotLongitudinalControl
+ ret.communityFeature = ret.openpilotLongitudinalControl
else:
ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
- ret.enableCamera = True
ret.enableGasInterceptor = 0x201 in fingerprint[0]
- ret.openpilotLongitudinalControl = ret.enableCamera
+ ret.openpilotLongitudinalControl = True
+
+ ret.pcmCruise = not ret.enableGasInterceptor
+ ret.communityFeature = ret.enableGasInterceptor
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
@@ -138,12 +151,6 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.ACCORD and 0x191 in fingerprint[1]:
ret.transmissionType = TransmissionType.cvt
- cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
- cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
-
- ret.enableCruise = not ret.enableGasInterceptor
- ret.communityFeature = ret.enableGasInterceptor
-
# Certain Hondas have an extra steering sensor at the bottom of the steering rack,
# which improves controls quality as it removes the steering column torsion from feedback.
# Tire stiffness factor fictitiously lower if it includes the steering column torsion effect.
@@ -408,7 +415,10 @@ class CarInterface(CarInterfaceBase):
# These cars use alternate user brake msg (0x1BE)
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
- ret.safetyParam = 1
+ ret.safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
+
+ if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
+ ret.safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
@@ -424,12 +434,17 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
- ret.gasMaxBP = [0.] # m/s
- ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
- ret.brakeMaxBP = [5., 20.] # m/s
- ret.brakeMaxV = [1., 0.8] # max brake allowed
+ if candidate in HONDA_BOSCH:
+ ret.gasMaxBP = [0.] # m/s
+ ret.gasMaxV = [0.6]
+ ret.brakeMaxBP = [0.] # m/s
+ ret.brakeMaxV = [1.] # max brake allowed, 3.5m/s^2
+ else:
+ ret.gasMaxBP = [0.] # m/s
+ ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
+ ret.brakeMaxBP = [5., 20.] # m/s
+ ret.brakeMaxV = [1., 0.8] # max brake allowed
- ret.stoppingControl = True
ret.startAccel = 0.5
ret.steerActuatorDelay = 0.1
@@ -438,6 +453,11 @@ class CarInterface(CarInterfaceBase):
return ret
+ @staticmethod
+ def init(CP, logcan, sendcan):
+ if CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl:
+ disable_radar(logcan, sendcan)
+
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
@@ -488,7 +508,7 @@ class CarInterface(CarInterfaceBase):
ret.buttonEvents = buttonEvents
# events
- events = self.create_common_events(ret, pcm_enable=self.CP.enableCruise)
+ events = self.create_common_events(ret, pcm_enable=False)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
@@ -496,18 +516,21 @@ class CarInterface(CarInterfaceBase):
if self.CS.park_brake:
events.add(EventName.parkBrake)
- if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
+ if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
- # it can happen that car cruise disables while comma system is enabled: need to
- # keep braking if needed or if the speed is very low
- if self.CP.enableCruise and not ret.cruiseState.enabled \
- and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl):
- # non loud alert if cruise disables below 25mph as expected (+ a little margin)
- if ret.vEgo < self.CP.minEnableSpeed + 2.:
- events.add(EventName.speedTooLow)
- else:
- events.add(EventName.cruiseDisabled)
+ if self.CP.pcmCruise:
+ # we engage when pcm is active (rising edge)
+ if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
+ events.add(EventName.pcmEnable)
+ elif not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl):
+ # it can happen that car cruise disables while comma system is enabled: need to
+ # keep braking if needed or if the speed is very low
+ if ret.vEgo < self.CP.minEnableSpeed + 2.:
+ # non loud alert if cruise disables below 25mph as expected (+ a little margin)
+ events.add(EventName.speedTooLow)
+ else:
+ events.add(EventName.cruiseDisabled)
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
@@ -516,7 +539,7 @@ class CarInterface(CarInterfaceBase):
# do enable on both accel and decel buttons
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
- if not self.CP.enableCruise:
+ if not self.CP.pcmCruise:
events.add(EventName.buttonEnable)
# do disable on button down
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 79bc28d29..289fab9f8 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -7,6 +7,8 @@ Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams():
+ ACCEL_MAX = 1.6
+
def __init__(self, CP):
self.BRAKE_MAX = 1024//4
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
@@ -16,6 +18,12 @@ class CarControllerParams():
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
+ self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6]
+ self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.]
+ self.BOSCH_GAS_LOOKUP_BP = [0., 0.6]
+ self.BOSCH_GAS_LOOKUP_V = [0, 2000]
+
+
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
@@ -28,6 +36,7 @@ VISUAL_HUD = {
VisualAlert.none: 0,
VisualAlert.fcw: 1,
VisualAlert.steerRequired: 1,
+ VisualAlert.ldw: 1,
VisualAlert.brakePressed: 10,
VisualAlert.wrongGear: 6,
VisualAlert.seatbeltUnbuckled: 5,
@@ -59,15 +68,9 @@ class CAR:
DIAG_MSGS = {1600: 5, 1601: 8}
FINGERPRINTS = {
- CAR.ACCORD: [{
- 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
- }],
CAR.ACCORDH: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
}],
- CAR.ACURA_ILX: [{
- 57: 3, 145: 8, 228: 5, 304: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 419: 8, 420: 8, 422: 8, 428: 8, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1030: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5,
- }],
# Acura RDX w/ Added Comma Pedal Support (512L & 513L)
CAR.ACURA_RDX: [{
57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1
@@ -86,9 +89,6 @@ FINGERPRINTS = {
CAR.CRV_5G: [{
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
}],
- CAR.FIT: [{
- 57: 3, 145: 8, 228: 5, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 422: 8, 427: 3, 428: 8, 432: 7, 464: 8, 487: 4, 490: 8, 506: 8, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8
- }],
# 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L)
CAR.ODYSSEY: [{
57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5
@@ -100,10 +100,6 @@ FINGERPRINTS = {
CAR.ODYSSEY_CHN: [{
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
}],
- # 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L)
- CAR.PILOT: [{
- 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5
- }],
# this fingerprint also includes the Passport 2019
CAR.PILOT_2019: [{
57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
@@ -156,6 +152,7 @@ FW_VERSIONS = {
b'28102-6B8-A570\x00\x00',
b'28102-6B8-A700\x00\x00',
b'28102-6B8-A800\x00\x00',
+ b'28102-6B8-C560\x00\x00',
b'28102-6B8-C570\x00\x00',
b'28102-6B8-M520\x00\x00',
b'28101-6A7-A220\x00\x00',
@@ -624,6 +621,7 @@ FW_VERSIONS = {
},
CAR.CRV_5G: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-5PA-AH20\x00\x00',
b'37805-5PA-3060\x00\x00',
b'37805-5PA-3080\x00\x00',
b'37805-5PA-3180\x00\x00',
@@ -1138,6 +1136,7 @@ FW_VERSIONS = {
b'78109-T6Z-A420\x00\x00',
b'78109-T6Z-A510\x00\x00',
b'78109-T6Z-A710\x00\x00',
+ b'78109-T6Z-A910\x00\x00',
b'78109-T6Z-AA10\x00\x00',
b'78109-T6Z-C620\x00\x00',
b'78109-TJZ-A510\x00\x00',
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 5f7019fdf..2bfc40425 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -10,7 +10,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
right_lane, left_lane_depart, right_lane_depart):
- sys_warning = (visual_alert == VisualAlert.steerRequired)
+ sys_warning = (visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw])
# initialize to no line visible
sys_state = 1
@@ -49,12 +49,8 @@ class CarController():
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
- # disable when temp fault is active
- lkas_active = enabled and not CS.out.steerWarning
-
- # fix for Genesis hard fault at low speed
- if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
- lkas_active = False
+ # disable when temp fault is active, or below LKA minimum speed
+ lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
if not lkas_active:
apply_steer = 0
@@ -82,7 +78,7 @@ class CarController():
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KONA_EV,
- CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021]:
+ CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
return can_sends
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 868838827..2cb176aa1 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -1,14 +1,25 @@
import copy
from cereal import car
-from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_HYBRID
+from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
+from opendbc.can.can_define import CANDefine
from selfdrive.config import Conversions as CV
-GearShifter = car.CarState.GearShifter
-
class CarState(CarStateBase):
+ def __init__(self, CP):
+ super().__init__(CP)
+ can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
+
+ if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
+ self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
+ elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
+ self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
+ else: # preferred and elect gear methods use same definition
+ self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
+
+
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@@ -29,8 +40,8 @@ class CarState(CarStateBase):
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
- ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
- cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
+ ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
+ 50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
@@ -40,7 +51,7 @@ class CarState(CarStateBase):
if self.CP.openpilotLongitudinalControl:
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
- ret.cruiseState.standstill = cp.vl["TCS13"]["StandStill"] == 1
+ ret.cruiseState.standstill = False
else:
ret.cruiseState.available = cp.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp.vl["SCC12"]["ACCMode"] != 0
@@ -56,64 +67,28 @@ class CarState(CarStateBase):
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
- if self.CP.carFingerprint in EV_HYBRID:
- ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 256.
+ if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
+ if self.CP.carFingerprint in HYBRID_CAR:
+ ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
+ else:
+ ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
- ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100
+ ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
- # TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
- if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
- ret.gearShifter = GearShifter.drive
- elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
- ret.gearShifter = GearShifter.neutral
- elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
- ret.gearShifter = GearShifter.park
- elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
- ret.gearShifter = GearShifter.reverse
- else:
- ret.gearShifter = GearShifter.unknown
- # Gear Selecton via TCU12
+ gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
- if gear == 0:
- ret.gearShifter = GearShifter.park
- elif gear == 14:
- ret.gearShifter = GearShifter.reverse
- elif gear > 0 and gear < 9: # unaware of anything over 8 currently
- ret.gearShifter = GearShifter.drive
- else:
- ret.gearShifter = GearShifter.unknown
- # Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
- if gear in (5, 8): # 5: D, 8: sport mode
- ret.gearShifter = GearShifter.drive
- elif gear == 6:
- ret.gearShifter = GearShifter.neutral
- elif gear == 0:
- ret.gearShifter = GearShifter.park
- elif gear == 7:
- ret.gearShifter = GearShifter.reverse
- else:
- ret.gearShifter = GearShifter.unknown
- # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
- if gear in (5, 8): # 5: D, 8: sport mode
- ret.gearShifter = GearShifter.drive
- elif gear == 6:
- ret.gearShifter = GearShifter.neutral
- elif gear == 0:
- ret.gearShifter = GearShifter.park
- elif gear == 7:
- ret.gearShifter = GearShifter.reverse
- else:
- ret.gearShifter = GearShifter.unknown
+
+ ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if self.CP.carFingerprint in FEATURES["use_fca"]:
ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
@@ -228,10 +203,15 @@ class CarState(CarStateBase):
]
checks += [("LCA11", 50)]
- if CP.carFingerprint in EV_HYBRID:
- signals += [
- ("Accel_Pedal_Pos", "E_EMS11", 0),
- ]
+ if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
+ if CP.carFingerprint in HYBRID_CAR:
+ signals += [
+ ("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0)
+ ]
+ else:
+ signals += [
+ ("Accel_Pedal_Pos", "E_EMS11", 0)
+ ]
checks += [
("E_EMS11", 50),
]
@@ -247,10 +227,7 @@ class CarState(CarStateBase):
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
- ("CF_Clu_InhibitD", "CLU15", 0),
- ("CF_Clu_InhibitP", "CLU15", 0),
- ("CF_Clu_InhibitN", "CLU15", 0),
- ("CF_Clu_InhibitR", "CLU15", 0),
+ ("CF_Clu_Gear", "CLU15", 0),
]
checks += [
("CLU15", 5)
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index 23645a125..56ae8e409 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -17,7 +17,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_MsgCount"] = frame % 0x10
- if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.KIA_SELTOS, CAR.ELANTRA_2021]:
+ if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index c4ff372df..6f643f118 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.config import Conversions as CV
-from selfdrive.car.hyundai.values import CAR
+from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@@ -77,6 +77,14 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.65
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+ elif candidate == CAR.ELANTRA_HEV_2021:
+ ret.lateralTuning.pid.kf = 0.00005
+ ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
+ ret.wheelbase = 2.72
+ ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
+ tire_stiffness_factor = 0.65
+ ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.HYUNDAI_GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
@@ -108,7 +116,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
- elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020]:
+ elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
@@ -116,7 +124,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
- if candidate != CAR.IONIQ_EV_2020:
+ if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.VELOSTER:
ret.lateralTuning.pid.kf = 0.00005
@@ -135,7 +143,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
- elif candidate == CAR.KIA_NIRO_EV:
+ elif candidate in [CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1737. + STD_CARGO_KG
ret.wheelbase = 2.7
@@ -143,6 +151,8 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+ if candidate == CAR.KIA_NIRO_HEV:
+ ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.KIA_SELTOS:
ret.mass = 1337. + STD_CARGO_KG
ret.wheelbase = 2.63
@@ -220,11 +230,17 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
# these cars require a special panda safety mode due to missing counters and checksums in the messages
- if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
+ if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.KIA_SELTOS,
CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA]:
ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
+ # set appropriate safety param for gas signal
+ if candidate in HYBRID_CAR:
+ ret.safetyParam = 2
+ elif candidate in EV_CAR:
+ ret.safetyParam = 1
+
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
@@ -236,7 +252,6 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
- ret.enableCamera = True
ret.enableBsm = 0x58b in fingerprint[0]
return ret
@@ -250,7 +265,6 @@ class CarInterface(CarInterfaceBase):
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret)
- # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index c69789598..f3fed2b19 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class CarControllerParams:
def __init__(self, CP):
- if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021]:
+ if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
self.STEER_MAX = 384
else:
self.STEER_MAX = 255
@@ -22,11 +22,13 @@ class CAR:
# Hyundai
ELANTRA = "HYUNDAI ELANTRA 2017"
ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
+ ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021"
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019"
IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020"
+ IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
SANTA_FE = "HYUNDAI SANTA FE 2019"
@@ -38,6 +40,7 @@ class CAR:
# Kia
KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
KIA_NIRO_EV = "KIA NIRO EV 2020"
+ KIA_NIRO_HEV = "KIA NIRO HYBRID 2019"
KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_SELTOS = "KIA SELTOS 2021"
@@ -153,6 +156,23 @@ FINGERPRINTS = {
FW_VERSIONS = {
+ CAR.IONIQ_PHEV: {
+ (Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ',
+ ],
+ (Ecu.eps, 0x7d4, None): [
+ b'\xf1\000AE MDPS C 1.00 1.01 56310/G2510 4APHC101',
+ ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\000AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x816H6F6051\000\000\000\000\000\000\000\000',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2',
+ ],
+ },
CAR.IONIQ_EV_2020: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00AEev SCC F-CUP 1.00 1.01 99110-G7000 ',
@@ -195,6 +215,7 @@ FW_VERSIONS = {
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00DN ESC \a 106 \a\x01 58910-L0100',
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02',
+ b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300',
b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100',
b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300\xf1\xa01.03',
@@ -206,6 +227,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x81HM6M1_0a0_F00',
b'\xf1\x82DNBVN5GMCCXXXDCA',
+ b'\xf1\x82DNBWN5TMDCXXXG2E',
b'\xf1\x82DNCVN5GMCCXXXG2B',
b'\xf1\x87391162M003\xf1\xa0000F',
b'\xf1\x87391162M003\xf1\xa00240',
@@ -217,6 +239,7 @@ FW_VERSIONS = {
b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101',
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101',
+ b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101\xf1\xa01.01',
b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01',
b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101\xf1\xa01.01',
@@ -240,6 +263,7 @@ FW_VERSIONS = {
b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92',
b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:',
+ b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
@@ -327,6 +351,7 @@ FW_VERSIONS = {
b'\xf1\x87SBLWAA6622844GG0wwwwff\x86hwwwwx\x88\x87\x88\x88\x88\x88\x88\x98?\xfd\xff\xa9\x88\x7f\xffn\xe5\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7u\x1e{\x1c',
b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2\x00\x00\x00\x00',
b'\xf1\x87SDKXAA2443414GG1vfvgwv\x87h\x88\x88\x88\x88ww\x87wwwww\x99_\xfc\xffvD?\xffl\xd2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4G24NS6\x00\x00\x00\x00',
+ b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00',
],
},
CAR.KIA_STINGER: {
@@ -360,13 +385,6 @@ FW_VERSIONS = {
b'\xf1\x87WAJTE17552812CH4vfFffvfVeT5DwvvVVdFeegeg\x88\x88o\xff\x1a]\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00TCK2T20NB1\x19\xd2\x00\x94',
],
},
- CAR.KIA_OPTIMA_H: {
- (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ',],
- (Ecu.engine, 0x7e0, None): [b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00',],
- (Ecu.eps, 0x7d4, None): [b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109',],
- (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424',],
- (Ecu.transmission, 0x7e1, None): [b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91",],
- },
CAR.PALISADE: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\000LX2_ SCC F-CUP 1.00 1.05 99110-S8100 \xf1\xa01.05',
@@ -431,14 +449,24 @@ FW_VERSIONS = {
],
},
CAR.VELOSTER: {
- (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ', ],
+ (Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ',
+ b'\xf1\x00JS__ SCC HNCUP 1.00 1.02 95650-J3100 ',
+ ],
(Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ],
- (Ecu.engine, 0x7e0, None): [b'\x01TJS-JNU06F200H0A', ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\x01TJS-JNU06F200H0A',
+ b'\x01TJS-JDK06F200H0A',
+ ],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ],
- (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32',
+ b'\xf1\x00JS LKAS AT KOR LHD 1.00 1.03 95740-J3000 K33',
+ ],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80',
b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00',
+ b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16KS2\016\xba\036\xa2',
],
},
CAR.GENESIS_G70: {
@@ -535,6 +563,24 @@ FW_VERSIONS = {
b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40',
],
},
+ CAR.KIA_NIRO_HEV: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x816H6F4051\000\000\000\000\000\000\000\000',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b"\xf1\x816U3J2051\000\000\xf1\0006U3H0_C2\000\0006U3J2051\000\000PDE0G16NS2\xf4\'\\\x91",
+ b'\xf1\x816U3J2051\000\000\xf1\0006U3H0_C2\000\0006U3J2051\000\000PDE0G16NS2\000\000\000\000',
+ ],
+ (Ecu.eps, 0x7D4, None): [
+ b'\xf1\000DE MDPS C 1.00 1.09 56310G5301\000 4DEHC109',
+ ],
+ (Ecu.fwdCamera, 0x7C4, None): [
+ b'\xf1\000DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424',
+ ],
+ (Ecu.fwdRadar, 0x7D0, None): [
+ b'\xf1\000DEhe SCC H-CUP 1.01 1.02 96400-G5100 ',
+ ],
+ },
CAR.KIA_SELTOS: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 \xf1\xa01.05',],
(Ecu.esp, 0x7d1, None): [
@@ -543,6 +589,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x81616D2051\000\000\000\000\000\000\000\000',
+ b'\xf1\x81616D5051\000\000\000\000\000\000\000\000',
b'\001TSP2KNL06F100J0K',
b'\001TSP2KNL06F200J0K',
],
@@ -588,11 +635,28 @@ FW_VERSIONS = {
b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\177\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [b'\xf1\x82CNCWD0AMFCXCSFFA'],
+ },
+ CAR.ELANTRA_HEV_2021: {
+ (Ecu.fwdCamera, 0x7c4, None) : [
+ b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819'
+ ],
+ (Ecu.fwdRadar, 0x7d0, None) : [
+ b'\xf1\000CNhe SCC FHCUP 1.00 1.01 99110-BY000 '
+ ],
+ (Ecu.eps, 0x7d4, None) :[
+ b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102\xf1\xa01.02'
+ ],
+ (Ecu.transmission, 0x7e1, None) :[
+ b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa'
+ ],
+ (Ecu.engine, 0x7e0, None) : [
+ b'\xf1\x816H6G5051\000\000\000\000\000\000\000\000'
+ ]
}
}
CHECKSUM = {
- "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021],
+ "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
@@ -600,27 +664,31 @@ FEATURES = {
# which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]),
- "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020]),
+ "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021]),
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
- "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS]),
+ "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS]),
}
-EV_HYBRID = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
+HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV]) # these cars use a different gas signal
+EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
DBC = {
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
+ CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
+ CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
+ CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index f049434bf..34c6c5a54 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -27,6 +27,8 @@ class CarInterfaceBase():
self.VM = VehicleModel(CP)
self.frame = 0
+ self.steer_warning = 0
+ self.steering_unpressed = 0
self.low_speed_alert = False
if CarState is not None:
@@ -51,6 +53,10 @@ class CarInterfaceBase():
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
raise NotImplementedError
+ @staticmethod
+ def init(CP, logcan, sendcan):
+ pass
+
# returns a set of default params to avoid repetition in car specific params
@staticmethod
def get_std_params(candidate, fingerprint):
@@ -63,12 +69,11 @@ class CarInterfaceBase():
ret.steerMaxV = [1.]
ret.minSteerSpeed = 0.
- # stock ACC by default
- ret.enableCruise = True
- ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
+ ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
+ ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
ret.gasMaxBP = [0.]
- ret.gasMaxV = [.5] # half max brake
+ ret.gasMaxV = [.5] # half max brake
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.openpilotLongitudinalControl = False
@@ -76,7 +81,7 @@ class CarInterfaceBase():
ret.minSpeedCan = 0.3
ret.stoppingBrakeRate = 0.2 # brake_travel/s while trying to stop
ret.startingBrakeRate = 0.8 # brake_travel/s while releasing on restart
- ret.stoppingControl = False
+ ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.longitudinalTuning.kpBP = [0.]
@@ -93,14 +98,15 @@ class CarInterfaceBase():
def apply(self, c):
raise NotImplementedError
- def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1, pcm_enable=True): # pylint: disable=dangerous-default-value
+ def create_common_events(self, cs_out, extra_gears=None, gas_resume_speed=-1, pcm_enable=True):
events = Events()
if cs_out.doorOpen:
events.add(EventName.doorOpen)
if cs_out.seatbeltUnlatched:
events.add(EventName.seatbeltNotLatched)
- if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears:
+ if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
+ cs_out.gearShifter not in extra_gears):
events.add(EventName.wrongGear)
if cs_out.gearShifter == GearShifter.reverse:
events.add(EventName.reverseGear)
@@ -119,13 +125,19 @@ class CarInterfaceBase():
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
+ self.steer_warning = self.steer_warning + 1 if cs_out.steerWarning else 0
+ self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
+
+ # Handle permanent and temporary steering faults
if cs_out.steerError:
events.add(EventName.steerUnavailable)
elif cs_out.steerWarning:
- if cs_out.steeringPressed:
- events.add(EventName.steerTempUnavailableUserOverride)
- else:
+ # only escalate to the harsher alert after the condition has
+ # persisted for 0.5s and we're certain that the user isn't overriding
+ if self.steering_unpressed > int(0.5/DT_CTRL) and self.steer_warning > int(0.5/DT_CTRL):
events.add(EventName.steerTempUnavailable)
+ else:
+ events.add(EventName.steerTempUnavailableSilent)
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
# Optionally allow to press gas at zero speed to resume.
@@ -167,6 +179,8 @@ class CarStateBase:
self.cruise_buttons = 0
self.left_blinker_cnt = 0
self.right_blinker_cnt = 0
+ self.left_blinker_prev = False
+ self.right_blinker_prev = False
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
@@ -182,11 +196,37 @@ class CarStateBase:
v_ego_x = self.v_ego_kf.update(v_ego_raw)
return float(v_ego_x[0]), float(v_ego_x[1])
- def update_blinker(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
+ def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
+ """Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
+ iterations"""
+ # TODO: Handle case when switching direction. Now both blinkers can be on at the same time
self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0)
self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0)
return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0
+ def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool):
+ """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time,
+ or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker
+ is forced to the other side. On a rising edge of the stalk the timeout is reset."""
+
+ if left_blinker_stalk:
+ self.right_blinker_cnt = 0
+ if not self.left_blinker_prev:
+ self.left_blinker_cnt = blinker_time
+
+ if right_blinker_stalk:
+ self.left_blinker_cnt = 0
+ if not self.right_blinker_prev:
+ self.right_blinker_cnt = blinker_time
+
+ self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0)
+ self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0)
+
+ self.left_blinker_prev = left_blinker_stalk
+ self.right_blinker_prev = right_blinker_stalk
+
+ return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
+
@staticmethod
def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
d: Dict[str, car.CarState.GearShifter] = {
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index c79ff9b3a..dfc1c12bf 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -20,11 +20,11 @@ class CarInterface(CarInterfaceBase):
ret.carName = "mazda"
ret.safetyModel = car.CarParams.SafetyModel.mazda
-
- ret.dashcamOnly = True
-
ret.radarOffCan = True
+ ret.communityFeature = True
+ ret.dashcamOnly = True
+
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 1.0
ret.steerLimitTimer = 0.8
@@ -37,7 +37,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]]
ret.lateralTuning.pid.kf = 0.00006
- elif candidate == CAR.CX9:
+ elif candidate in [CAR.CX9, CAR.CX9_2021]:
ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 3.1
ret.steerRatio = 17.6
@@ -73,8 +73,6 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
- ret.enableCamera = True
-
return ret
# returns a car.CarState
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 6ad1da04a..c49b8c86e 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -8,19 +8,20 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class CarControllerParams:
- STEER_MAX = 600 # max_steer 2048
- STEER_STEP = 1 # how often we update the steer cmd
+ STEER_MAX = 800 # theoretical max_steer 2047
STEER_DELTA_UP = 10 # torque increase per refresh
- STEER_DELTA_DOWN = 20 # torque decrease per refresh
+ STEER_DELTA_DOWN = 25 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 1 # weight driver torque
STEER_DRIVER_FACTOR = 1 # from dbc
+ STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
class CAR:
CX5 = "MAZDA CX-5"
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
+ CX9_2021 = "Mazda CX-9 2021" # No Steer Lockout
class LKAS_LIMITS:
STEER_THRESHOLD = 15
@@ -39,19 +40,25 @@ FW_VERSIONS = {
CAR.CX5: {
(Ecu.eps, 0x730, None): [
b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'KJ01-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-M-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYFA-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYFC-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYFD-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX2H-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX38-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX42-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX68-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
@@ -64,14 +71,20 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x706, None): [
b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-R\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'GSH7-67XK2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PX39-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYB1-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYB1-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX68-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
- b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYB2-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYB2-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYB2-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYB2-21PS1-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
@@ -79,44 +92,52 @@ FW_VERSIONS = {
CAR.CX9 : {
(Ecu.eps, 0x730, None): [
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KJ01-3210X-L-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX23-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PX24-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXN8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x760, None): [
b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'TK80-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'B61L-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'B61L-67XK2-V\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'GSH7-67XK2-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PYD6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAZDA3: {
(Ecu.eps, 0x730, None): [
- b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'KR11-3210X-K-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'P5JD-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PYKC-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'PY2P-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -156,6 +177,27 @@ FW_VERSIONS = {
(Ecu.transmission, 0x7e1, None): [
b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
+ },
+
+ CAR.CX9_2021 : {
+ (Ecu.eps, 0x730, None): [
+ b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x764, None): [
+ b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.esp, 0x760, None): [
+ b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x706, None): [
+ b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
}
}
@@ -165,6 +207,11 @@ DBC = {
CAR.CX9: dbc_dict('mazda_2017', None),
CAR.MAZDA3: dbc_dict('mazda_2017', None),
CAR.MAZDA6: dbc_dict('mazda_2017', None),
+ CAR.CX9_2021: dbc_dict('mazda_2017', None),
}
-GEN1 = [ CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6 ]
+# Gen 1 hardware: same CAN messages and same camera
+GEN1 = set([CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6])
+
+# Cars with a steering lockout
+STEER_LOCKOUT_CAR = set([CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6])
diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py
index 8f763f358..41ba9f659 100644
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/selfdrive/car/nissan/carcontroller.py
@@ -31,7 +31,7 @@ class CarController():
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steeringAngleDeg
- steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
+ steer_hud_alert = 1 if hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw] else 0
if enabled:
# # windup slower
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index 30f4e94f9..d85abd770 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -24,7 +24,6 @@ class CarInterface(CarInterfaceBase):
ret.communityFeature = True
ret.steerLimitAlert = False
- ret.enableCamera = True
ret.steerRateCost = 0.5
ret.steerActuatorDelay = 0.1
@@ -46,7 +45,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.824
ret.centerToFront = ret.wheelbase * 0.44
ret.steerRatio = 17
-
+
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.radarOffCan = True
diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py
index 8ec957a65..80ac2542a 100644
--- a/selfdrive/car/subaru/carcontroller.py
+++ b/selfdrive/car/subaru/carcontroller.py
@@ -15,7 +15,7 @@ class CarController():
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
- def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
+ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
can_sends = []
@@ -69,7 +69,7 @@ class CarController():
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
- can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
+ can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index 888c370eb..d2fa0c795 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -33,8 +33,8 @@ class CarState(CarStateBase):
ret.standstill = ret.vEgoRaw < 0.01
# continuous blinker signals for assisted lane change
- ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
- cp.vl["Dashlights"]["RIGHT_BLINKER"])
+ ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
+ 50, cp.vl["Dashlights"]["LEFT_BLINKER"], cp.vl["Dashlights"]["RIGHT_BLINKER"])
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index bb8669fb1..436dda8d1 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -28,8 +28,6 @@ class CarInterface(CarInterfaceBase):
ret.communityFeature = True
ret.dashcamOnly = candidate in PREGLOBAL_CARS
- ret.enableCamera = True
-
ret.steerRateCost = 0.7
ret.steerLimitTimer = 0.4
@@ -122,6 +120,6 @@ class CarInterface(CarInterfaceBase):
def apply(self, c):
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
c.cruiseControl.cancel, c.hudControl.visualAlert,
- c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible)
+ c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
self.frame += 1
return can_sends
diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py
index 672b6873b..5a98ea554 100644
--- a/selfdrive/car/subaru/subarucan.py
+++ b/selfdrive/car/subaru/subarucan.py
@@ -27,12 +27,20 @@ def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd):
return packer.make_can_msg("ES_Distance", 0, values)
-def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line):
+def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
values = copy.copy(es_lkas_msg)
if visual_alert == VisualAlert.steerRequired:
values["Keep_Hands_On_Wheel"] = 1
+ # Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW)
+ if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0:
+ if left_lane_depart:
+ values["LKAS_Alert"] = 12 # Left lane departure dash alert
+
+ elif right_lane_depart:
+ values["LKAS_Alert"] = 11 # Right lane departure dash alert
+
values["LKAS_Left_Line_Visible"] = int(left_line)
values["LKAS_Right_Line_Visible"] = int(right_line)
diff --git a/selfdrive/controls/lib/longitudinal_mpc/__init__.py b/selfdrive/car/tesla/__init__.py
similarity index 100%
rename from selfdrive/controls/lib/longitudinal_mpc/__init__.py
rename to selfdrive/car/tesla/__init__.py
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
new file mode 100644
index 000000000..df53b9dd9
--- /dev/null
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -0,0 +1,53 @@
+from common.numpy_fast import clip, interp
+from selfdrive.car.tesla.teslacan import TeslaCAN
+from opendbc.can.packer import CANPacker
+from selfdrive.car.tesla.values import CANBUS, CarControllerParams
+
+class CarController():
+ def __init__(self, dbc_name, CP, VM):
+ self.CP = CP
+ self.last_angle = 0
+ self.packer = CANPacker(dbc_name)
+ self.tesla_can = TeslaCAN(dbc_name, self.packer)
+
+ def update(self, enabled, CS, frame, actuators, cruise_cancel):
+ can_sends = []
+
+ # Temp disable steering on a hands_on_fault, and allow for user override
+ hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
+ lkas_enabled = enabled and (not hands_on_fault)
+
+ if lkas_enabled:
+ apply_angle = actuators.steeringAngleDeg
+
+ # Angular rate limit based on speed
+ steer_up = (self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle))
+ rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
+ max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
+ apply_angle = clip(apply_angle, (self.last_angle - max_angle_diff), (self.last_angle + max_angle_diff))
+
+ # To not fault the EPS
+ apply_angle = clip(apply_angle, (CS.out.steeringAngleDeg - 20), (CS.out.steeringAngleDeg + 20))
+ else:
+ apply_angle = CS.out.steeringAngleDeg
+
+ self.last_angle = apply_angle
+ can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame))
+
+ # Cancel on user steering override, since there is no steering torque blending
+ if hands_on_fault:
+ cruise_cancel = True
+
+ # Cancel when openpilot is not enabled anymore
+ if not enabled and bool(CS.out.cruiseState.enabled):
+ cruise_cancel = True
+
+ if ((frame % 10) == 0 and cruise_cancel):
+ # Spam every possible counter value, otherwise it might not be accepted
+ for counter in range(16):
+ can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.chassis, counter))
+ can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot, counter))
+
+ # TODO: HUD control
+
+ return can_sends
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
new file mode 100644
index 000000000..204152611
--- /dev/null
+++ b/selfdrive/car/tesla/carstate.py
@@ -0,0 +1,181 @@
+import copy
+from cereal import car
+from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
+from selfdrive.car.interfaces import CarStateBase
+from opendbc.can.parser import CANParser
+from opendbc.can.can_define import CANDefine
+from selfdrive.config import Conversions as CV
+
+class CarState(CarStateBase):
+ def __init__(self, CP):
+ super().__init__(CP)
+ self.button_states = {button.event_type: False for button in BUTTONS}
+ self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis'])
+
+ # Needed by carcontroller
+ self.msg_stw_actn_req = None
+ self.hands_on_level = 0
+ self.steer_warning = None
+
+ def update(self, cp, cp_cam):
+ ret = car.CarState.new_message()
+
+ # Vehicle speed
+ ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
+ ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+ ret.standstill = (ret.vEgo < 0.1)
+
+ # Gas pedal
+ ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0
+ ret.gasPressed = (ret.gas > 0)
+
+ # Brake pedal
+ ret.brake = 0
+ ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1)
+
+ # Steering wheel
+ self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"]
+ self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None)
+ steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None)
+
+ ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"]
+ ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
+ ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
+ ret.steeringPressed = (self.hands_on_level > 0)
+ ret.steerError = steer_status == "EAC_FAULT"
+ ret.steerWarning = self.steer_warning in ["EAC_ERROR_MAX_SPEED", "EAC_ERROR_MIN_SPEED", "EAC_ERROR_TMP_FAULT", "SNA"] # TODO: not sure if this list is complete
+
+ # Cruise state
+ cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
+ speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None)
+
+ acc_enabled = (cruise_state in ["ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"])
+
+ ret.cruiseState.enabled = acc_enabled
+ if speed_units == "KPH":
+ ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
+ elif speed_units == "MPH":
+ ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
+ ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled)
+ ret.cruiseState.standstill = (cruise_state == "STANDSTILL")
+
+ # Gear
+ ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
+
+ # Buttons
+ buttonEvents = []
+ for button in BUTTONS:
+ state = (cp.vl[button.can_addr][button.can_msg] in button.values)
+ if self.button_states[button.event_type] != state:
+ event = car.CarState.ButtonEvent.new_message()
+ event.type = button.event_type
+ event.pressed = state
+ buttonEvents.append(event)
+ self.button_states[button.event_type] = state
+ ret.buttonEvents = buttonEvents
+
+ # Doors
+ ret.doorOpen = any([(self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS])
+
+ # Blinkers
+ ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1)
+ ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1)
+
+ # Seatbelt
+ ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
+
+ # TODO: blindspot
+
+ # Messages needed by carcontroller
+ self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
+
+ return ret
+
+ @staticmethod
+ def get_can_parser(CP):
+ signals = [
+ # sig_name, sig_address, default
+ ("ESP_vehicleSpeed", "ESP_B", 0),
+ ("DI_pedalPos", "DI_torque1", 0),
+ ("DI_brakePedal", "DI_torque2", 0),
+ ("StW_AnglHP", "STW_ANGLHP_STAT", 0),
+ ("StW_AnglHP_Spd", "STW_ANGLHP_STAT", 0),
+ ("EPAS_handsOnLevel", "EPAS_sysStatus", 0),
+ ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0),
+ ("EPAS_internalSAS", "EPAS_sysStatus", 0),
+ ("EPAS_eacStatus", "EPAS_sysStatus", 1),
+ ("EPAS_eacErrorCode", "EPAS_sysStatus", 0),
+ ("DI_cruiseState", "DI_state", 0),
+ ("DI_digitalSpeed", "DI_state", 0),
+ ("DI_speedUnits", "DI_state", 0),
+ ("DI_gear", "DI_torque2", 0),
+ ("DOOR_STATE_FL", "GTW_carState", 1),
+ ("DOOR_STATE_FR", "GTW_carState", 1),
+ ("DOOR_STATE_RL", "GTW_carState", 1),
+ ("DOOR_STATE_RR", "GTW_carState", 1),
+ ("DOOR_STATE_FrontTrunk", "GTW_carState", 1),
+ ("BOOT_STATE", "GTW_carState", 1),
+ ("BC_indicatorLStatus", "GTW_carState", 1),
+ ("BC_indicatorRStatus", "GTW_carState", 1),
+ ("SDM_bcklDrivStatus", "SDM1", 0),
+ ("driverBrakeStatus", "BrakeMessage", 0),
+
+ # We copy this whole message when spamming cancel
+ ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0),
+ ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0),
+ ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ", 0),
+ ("DTR_Dist_Rq", "STW_ACTN_RQ", 0),
+ ("TurnIndLvr_Stat", "STW_ACTN_RQ", 0),
+ ("HiBmLvr_Stat", "STW_ACTN_RQ", 0),
+ ("WprWashSw_Psd", "STW_ACTN_RQ", 0),
+ ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ", 0),
+ ("StW_Lvr_Stat", "STW_ACTN_RQ", 0),
+ ("StW_Cond_Flt", "STW_ACTN_RQ", 0),
+ ("StW_Cond_Psd", "STW_ACTN_RQ", 0),
+ ("HrnSw_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw00_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw01_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw02_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw03_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw04_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw05_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw06_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw07_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw08_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw09_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw10_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw11_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw12_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw13_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw14_Psd", "STW_ACTN_RQ", 0),
+ ("StW_Sw15_Psd", "STW_ACTN_RQ", 0),
+ ("WprSw6Posn", "STW_ACTN_RQ", 0),
+ ("MC_STW_ACTN_RQ", "STW_ACTN_RQ", 0),
+ ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ", 0),
+ ]
+
+ checks = [
+ # sig_address, frequency
+ ("ESP_B", 50),
+ ("DI_torque1", 100),
+ ("DI_torque2", 100),
+ ("STW_ANGLHP_STAT", 100),
+ ("EPAS_sysStatus", 25),
+ ("DI_state", 10),
+ ("STW_ACTN_RQ", 10),
+ ("GTW_carState", 10),
+ ("SDM1", 10),
+ ("BrakeMessage", 50),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.chassis)
+
+ @staticmethod
+ def get_cam_can_parser(CP):
+ signals = [
+ # sig_name, sig_address, default
+ ]
+ checks = [
+ # sig_address, frequency
+ ]
+ return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot)
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
new file mode 100755
index 000000000..b3564c10c
--- /dev/null
+++ b/selfdrive/car/tesla/interface.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+from cereal import car
+from selfdrive.car.tesla.values import CAR
+from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
+from selfdrive.car.interfaces import CarInterfaceBase
+
+
+class CarInterface(CarInterfaceBase):
+ @staticmethod
+ def compute_gb(accel, speed):
+ # TODO: is this correct?
+ return accel
+
+ @staticmethod
+ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
+ ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
+ ret.carName = "tesla"
+ ret.safetyModel = car.CarParams.SafetyModel.tesla
+
+ # There is no safe way to do steer blending with user torque,
+ # so the steering behaves like autopilot. This is not
+ # how openpilot should be, hence dashcamOnly
+ ret.dashcamOnly = True
+
+ ret.steerControlType = car.CarParams.SteerControlType.angle
+ ret.openpilotLongitudinalControl = False
+ ret.communityFeature = True
+
+ ret.steerActuatorDelay = 0.1
+ ret.steerRateCost = 0.5
+
+ if candidate in [CAR.AP2_MODELS, CAR.AP1_MODELS]:
+ ret.mass = 2100. + STD_CARGO_KG
+ ret.wheelbase = 2.959
+ ret.centerToFront = ret.wheelbase * 0.5
+ ret.steerRatio = 13.5
+ else:
+ raise ValueError(f"Unsupported car: {candidate}")
+
+ ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+ ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
+
+ return ret
+
+ def update(self, c, can_strings):
+ self.cp.update_strings(can_strings)
+ self.cp_cam.update_strings(can_strings)
+
+ ret = self.CS.update(self.cp, self.cp_cam)
+ ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
+
+ events = self.create_common_events(ret)
+
+ ret.events = events.to_msg()
+ self.CS.out = ret.as_reader()
+ return self.CS.out
+
+ def apply(self, c):
+ can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
+ self.frame += 1
+ return can_sends
diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py
new file mode 100755
index 000000000..b2f765113
--- /dev/null
+++ b/selfdrive/car/tesla/radar_interface.py
@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+ pass
diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
new file mode 100644
index 000000000..0dee8b182
--- /dev/null
+++ b/selfdrive/car/tesla/teslacan.py
@@ -0,0 +1,41 @@
+import copy
+import crcmod
+from opendbc.can.can_define import CANDefine
+from selfdrive.car.tesla.values import CANBUS
+
+
+class TeslaCAN:
+ def __init__(self, dbc_name, packer):
+ self.can_define = CANDefine(dbc_name)
+ self.packer = packer
+ self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
+
+ @staticmethod
+ def checksum(msg_id, dat):
+ # TODO: get message ID from name instead
+ ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF)
+ ret += sum(dat)
+ return ret & 0xFF
+
+ def create_steering_control(self, angle, enabled, frame):
+ values = {
+ "DAS_steeringAngleRequest": -angle,
+ "DAS_steeringHapticRequest": 0,
+ "DAS_steeringControlType": 1 if enabled else 0,
+ "DAS_steeringControlCounter": (frame % 16),
+ }
+
+ data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2]
+ values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
+ return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)
+
+ def create_action_request(self, msg_stw_actn_req, cancel, bus, counter):
+ values = copy.copy(msg_stw_actn_req)
+
+ if cancel:
+ values["SpdCtrlLvr_Stat"] = 1
+ values["MC_STW_ACTN_RQ"] = counter
+
+ data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2]
+ values["CRC_STW_ACTN_RQ"] = self.crc(data[:7])
+ return self.packer.make_can_msg("STW_ACTN_RQ", bus, values)
diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py
new file mode 100644
index 000000000..2a7cf9e50
--- /dev/null
+++ b/selfdrive/car/tesla/values.py
@@ -0,0 +1,61 @@
+# flake8: noqa
+
+from collections import namedtuple
+from selfdrive.car import dbc_dict
+from cereal import car
+
+Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
+AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
+
+class CAR:
+ AP1_MODELS = 'TESLA AP1 MODEL S'
+ AP2_MODELS = 'TESLA AP2 MODEL S'
+
+FINGERPRINTS = {
+ CAR.AP2_MODELS: [
+ {
+ 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4
+ },
+ ],
+ CAR.AP1_MODELS: [
+ {
+ 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4
+ },
+ ],
+}
+
+DBC = {
+ CAR.AP2_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
+ CAR.AP1_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
+}
+
+class CANBUS:
+ chassis = 0
+ autopilot = 2
+ radar = 1
+
+GEAR_MAP = {
+ "DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
+ "DI_GEAR_P": car.CarState.GearShifter.park,
+ "DI_GEAR_R": car.CarState.GearShifter.reverse,
+ "DI_GEAR_N": car.CarState.GearShifter.neutral,
+ "DI_GEAR_D": car.CarState.GearShifter.drive,
+ "DI_GEAR_SNA": car.CarState.GearShifter.unknown,
+}
+
+DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"]
+
+# Make sure the message and addr is also in the CAN parser!
+BUTTONS = [
+ Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]),
+ Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]),
+ Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]),
+ Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]),
+ Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
+ Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
+]
+
+class CarControllerParams:
+ RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
+ RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
+
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 1bc18efb7..42c47f1b0 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -4,7 +4,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command,
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_accel_command, create_acc_cancel_command, \
create_fcw_command, create_lta_steer_command
-from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
+from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
from opendbc.can.packer import CANPacker
@@ -36,12 +36,6 @@ class CarController():
self.steer_rate_limited = False
self.use_interceptor = False
- self.fake_ecus = set()
- if CP.enableCamera:
- self.fake_ecus.add(Ecu.fwdCamera)
- if CP.enableDsu:
- self.fake_ecus.add(Ecu.dsu)
-
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
@@ -104,27 +98,26 @@ class CarController():
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
- if Ecu.fwdCamera in self.fake_ecus:
- can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
- if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
- can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
+ can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
+ if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
+ can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
- # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
- # if frame % 2 == 0:
- # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
- # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
+ # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
+ # if frame % 2 == 0:
+ # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
+ # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
- if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
+ if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
- can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead))
+ can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
else:
- can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
+ can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
@@ -135,7 +128,7 @@ class CarController():
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
- steer_alert = hud_alert == VisualAlert.steerRequired
+ steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
@@ -146,16 +139,16 @@ class CarController():
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
- if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
+ if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
- if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
+ if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
- for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
- if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
+ for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
+ if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 1f5cae59f..952e39ce2 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR
class CarState(CarStateBase):
@@ -20,6 +20,9 @@ class CarState(CarStateBase):
self.accurate_steer_angle_seen = False
self.angle_offset = 0.
+ self.low_speed_lockout = False
+ self.acc_type = 1
+
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@@ -74,11 +77,21 @@ class CarState(CarStateBase):
if self.CP.carFingerprint == CAR.LEXUS_IS:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
- self.low_speed_lockout = False
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
+
+ if self.CP.carFingerprint in TSS2_CAR:
+ self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
+
+ # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
+ # these cars are identified by an ACC_TYPE value of 2.
+ # TODO: it may be possible to avoid the lockout and gain stop and go if you
+ # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
+ if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
+ (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
+
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
@@ -183,7 +196,7 @@ class CarState(CarStateBase):
signals = [
("FORCE", "PRE_COLLISION", 0),
- ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)
+ ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0),
]
# use steering message to check if panda is connected to frc
@@ -192,4 +205,8 @@ class CarState(CarStateBase):
("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent
]
+ if CP.carFingerprint in TSS2_CAR:
+ signals.append(("ACC_TYPE", "ACC_CONTROL", 0))
+ checks.append(("ACC_CONTROL", 33))
+
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 6408ad01a..dd1543246 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -3,7 +3,6 @@ from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
-from selfdrive.swaglog import cloudlog
from selfdrive.car.interfaces import CarInterfaceBase
EventName = car.CarEvent.EventName
@@ -24,6 +23,8 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
+ ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
+
if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -166,36 +167,22 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
ret.lateralTuning.pid.kf = 0.00006
- elif candidate == CAR.RAV4_TSS2:
+ elif candidate in [CAR.RAV4_TSS2, CAR.RAV4H_TSS2]:
stop_and_go = True
ret.safetyParam = 73
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
tire_stiffness_factor = 0.7933
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]]
- ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kf = 0.00004
+ ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
+ ret.lateralTuning.pid.kf = 0.00007818594
+ # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
+ # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
for fw in car_fw:
- if fw.ecu == "eps" and fw.fwVersion == b"8965B42170\x00\x00\x00\x00\x00\x00":
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
- break
-
- elif candidate == CAR.RAV4H_TSS2:
- stop_and_go = True
- ret.safetyParam = 73
- ret.wheelbase = 2.68986
- ret.steerRatio = 14.3
- tire_stiffness_factor = 0.7933
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]]
- ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kf = 0.00004
-
- for fw in car_fw:
- if fw.ecu == "eps" and fw.fwVersion == b"8965B42170\x00\x00\x00\x00\x00\x00":
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
- ret.lateralTuning.pid.kf = 0.00007818594
+ if fw.ecu == "eps" and fw.fwVersion.startswith(b'\x02'):
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]]
+ ret.lateralTuning.pid.kf = 0.00004
break
elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
@@ -288,6 +275,16 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00006
+ elif candidate == CAR.ALPHARD_TSS2:
+ stop_and_go = True
+ ret.safetyParam = 73
+ ret.wheelbase = 3.00
+ ret.steerRatio = 14.2
+ tire_stiffness_factor = 0.444
+ ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]]
+ ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
+ ret.lateralTuning.pid.kf = 0.00007818594
+
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@@ -300,7 +297,6 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
- ret.enableCamera = True
ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
# Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it
smartDsu = 0x2FF in fingerprint[0]
@@ -309,9 +305,7 @@ class CarInterface(CarInterfaceBase):
ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR)
ret.enableGasInterceptor = 0x201 in fingerprint[0]
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
- ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
- cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
- cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
+ ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 14bc40744..1bb41f102 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,11 +28,11 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
return packer.make_can_msg("STEERING_LTA", 0, values)
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
- "SET_ME_X01": 1,
+ "ACC_TYPE": acc_type,
"DISTANCE": 0,
"MINI_CAR": lead,
"SET_ME_X3": 3,
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 080b70f61..92f6db919 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -10,8 +10,8 @@ PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
class CarControllerParams:
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
- ACCEL_MAX = 1.5 # 1.5 m/s2
- ACCEL_MIN = -3.0 # 3 m/s2
+ ACCEL_MAX = 1.5 # m/s2
+ ACCEL_MIN = -3.0 # m/s2
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
STEER_MAX = 1500
@@ -57,27 +57,28 @@ class CAR:
LEXUS_NX = "LEXUS NX 2018"
LEXUS_NX_TSS2 = "LEXUS NX 2020"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
+ ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
-# addr: (ecu, cars, bus, 1/freq*100, vl)
-STATIC_MSGS = [
- (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
- (0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
- (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
- (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
- (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
- (0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
- (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
- (0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
- (0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
- (0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
- (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
- (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
- (0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
- (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
- (0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
- (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
- (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'),
- (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
+# (addr, cars, bus, 1/freq*100, vl)
+STATIC_DSU_MSGS = [
+ (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
+ (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
+ (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
+ (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
+ (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
+ (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
+ (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
+ (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
+ (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
+ (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
+ (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
+ (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
+ (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
+ (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
+ (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
+ (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
+ (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'),
+ (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
]
@@ -119,22 +120,6 @@ FINGERPRINTS = {
CAR.COROLLA: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
}],
- CAR.LEXUS_RXH: [{
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
- },
- # RX450HL
- # TODO: get proper fingerprint in stock mode
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # RX540H 2019 with color hud
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
- },
- # 2017 RX 450h
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 744: 8, 767: 4, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
- }],
CAR.CAMRY: [
#XLE and LE
{
@@ -184,18 +169,6 @@ FINGERPRINTS = {
# 2019 Highlander Hybrid Limited Platinum
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
- CAR.AVALON: [{
- 36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- }],
- CAR.RAV4_TSS2: [
- # LE
- {
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767: 4, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # XLE, Limited, and AWD
- {
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
- }],
CAR.COROLLAH_TSS2: [
# 2019 Taiwan Altis Hybrid
{
@@ -214,18 +187,6 @@ FINGERPRINTS = {
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767: 4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
- CAR.LEXUS_IS: [
- # IS300 2018
- {
- 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767: 4, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # IS300H 2017
- {
- 36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767: 4, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- }],
- CAR.LEXUS_CTH: [{
- 36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- }],
}
@@ -285,15 +246,22 @@ FW_VERSIONS = {
CAR.AVALONH_2019: {
(Ecu.esp, 0x7b0, None): [
b'F152641040\x00\x00\x00\x00\x00\x00',
+ b'F152641061\x00\x00\x00\x00\x00\x00',
+ b'F152641050\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881510704200\x00\x00\x00\x00',
+ b'881514107100\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B07010\x00\x00\x00\x00\x00\x00',
+ b'8965B41090\x00\x00\x00\x00\x00\x00',
+ b'8965B41070\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+ b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
+ b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4702300\x00\x00\x00\x00',
@@ -475,6 +443,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x018966306Q5000\x00\x00\x00\x00',
b'\x018966306T3100\x00\x00\x00\x00',
+ b'\x018966306T4100\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 15): [
b'\x018821F6201200\x00\x00\x00\x00',
@@ -561,6 +530,7 @@ FW_VERSIONS = {
},
CAR.CHRH: {
(Ecu.engine, 0x700, None): [
+ b'\x02896631013200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@@ -569,12 +539,14 @@ FW_VERSIONS = {
],
(Ecu.esp, 0x7b0, None): [
b'F152610013\x00\x00\x00\x00\x00\x00',
+ b'F152610014\x00\x00\x00\x00\x00\x00',
b'F152610040\x00\x00\x00\x00\x00\x00',
b'F152610190\x00\x00\x00\x00\x00\x00',
b'F152610200\x00\x00\x00\x00\x00\x00',
b'F152610230\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
+ b'8821F0W01000 ',
b'8821FF402300 ',
b'8821FF402400 ',
b'8821FF404000 ',
@@ -588,6 +560,7 @@ FW_VERSIONS = {
b'8965B10050\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
+ b'8821F0W01000 ',
b'8821FF402300 ',
b'8821FF402400 ',
b'8821FF404000 ',
@@ -659,7 +632,6 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
- b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
@@ -676,7 +648,6 @@ FW_VERSIONS = {
b'\x018965B12530\x00\x00\x00\x00\x00\x00',
b'\x018965B1255000\x00\x00\x00\x00',
b'8965B12361\x00\x00\x00\x00\x00\x00',
- b'8965B58040\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152602280\x00\x00\x00\x00\x00\x00',
@@ -692,7 +663,6 @@ FW_VERSIONS = {
b'\x01F152612B90\x00\x00\x00\x00\x00\x00',
b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
b'F152602191\x00\x00\x00\x00\x00\x00',
- b'F152658320\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -709,7 +679,6 @@ FW_VERSIONS = {
b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
- b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.COROLLAH_TSS2: {
@@ -719,10 +688,12 @@ FW_VERSIONS = {
b'\x01896637621000\x00\x00\x00\x00',
b'\x01896637624000\x00\x00\x00\x00',
b'\x01896637626000\x00\x00\x00\x00',
+ b'\x01896637648000\x00\x00\x00\x00',
b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+ b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
@@ -731,6 +702,7 @@ FW_VERSIONS = {
b'8965B12361\x00\x00\x00\x00\x00\x00',
b'8965B12451\x00\x00\x00\x00\x00\x00',
b'8965B76012\x00\x00\x00\x00\x00\x00',
+ b'8965B76050\x00\x00\x00\x00\x00\x00',
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
b'\x018965B12470\x00\x00\x00\x00\x00\x00',
b'\x018965B12490\x00\x00\x00\x00\x00\x00',
@@ -752,6 +724,7 @@ FW_VERSIONS = {
b'F152642540\x00\x00\x00\x00\x00\x00',
b'F152676293\x00\x00\x00\x00\x00\x00',
b'F152676303\x00\x00\x00\x00\x00\x00',
+ b'F152676304\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
@@ -769,6 +742,7 @@ FW_VERSIONS = {
b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
+ b'\x028646F7603200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.HIGHLANDER: {
@@ -1386,6 +1360,7 @@ FW_VERSIONS = {
CAR.LEXUS_NX_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x018966378B2100\x00\x00\x00\x00',
+ b'\x018966378G3000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152678221\x00\x00\x00\x00\x00\x00',
@@ -1398,6 +1373,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
+ b'\x028646F7803100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.LEXUS_NXH: {
@@ -1405,6 +1381,7 @@ FW_VERSIONS = {
b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x0237880000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152678160\x00\x00\x00\x00\x00\x00',
@@ -1556,10 +1533,12 @@ FW_VERSIONS = {
CAR.LEXUS_RXH_TSS2: {
(Ecu.engine, 0x7e0, None): [
b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
+ b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152648831\x00\x00\x00\x00\x00\x00',
+ b'F152648D00\x00\x00\x00\x00\x00\x00',
b'F152648D60\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
@@ -1602,6 +1581,12 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F6201200\x00\x00\x00\x00',],
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',],
},
+ CAR.ALPHARD_TSS2: {
+ (Ecu.engine, 0x7e0, None): [b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',],
+ (Ecu.eps, 0x7a1, None): [b'8965B58040\x00\x00\x00\x00\x00\x00',],
+ (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301400\x00\x00\x00\x00',],
+ (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',],
+ },
}
STEER_THRESHOLD = 100
@@ -1643,13 +1628,14 @@ DBC = {
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.MIRAI: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
+ CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
}
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
- CAR.MIRAI, CAR.LEXUS_NX_TSS2])
+ CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2])
NO_DSU_CAR = TSS2_CAR | set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH])
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 503583f66..d816ba397 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -1,15 +1,16 @@
from cereal import car
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.volkswagen import volkswagencan
-from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams
+from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P
from opendbc.can.packer import CANPacker
+VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
- self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt'])
+ self.packer_pt = CANPacker(DBC_FILES.mqb)
self.hcaSameTorqueCount = 0
self.hcaEnabledFrameCount = 0
@@ -23,64 +24,35 @@ class CarController():
def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
""" Controls thread """
- P = CarControllerParams
-
- # Send CAN commands.
can_sends = []
- #--------------------------------------------------------------------------
- # #
- # Prepare HCA_01 Heading Control Assist messages with steering torque. #
- # #
- #--------------------------------------------------------------------------
+ # **** Steering Controls ************************************************ #
- # The factory camera sends at 50Hz while steering and 1Hz when not. When
- # OP is active, Panda filters HCA_01 from the factory camera and OP emits
- # HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and
- # doesn't seem to add value at this time. The rack will accept HCA_01 at
- # 100Hz if we want to control at finer resolution in the future.
if frame % P.HCA_STEP == 0:
+ # Logic to avoid HCA state 4 "refused":
+ # * Don't steer unless HCA is in state 3 "ready" or 5 "active"
+ # * Don't steer at standstill
+ # * Don't send > 3.00 Newton-meters torque
+ # * Don't send the same torque for > 6 seconds
+ # * Don't send uninterrupted steering for > 360 seconds
+ # One frame of HCA disabled is enough to reset the timer, without zeroing the
+ # torque value. Do that anytime we happen to have 0 torque, or failing that,
+ # when exceeding ~1/3 the 360 second timer.
- # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
- # commanding HCA if there's a fault, so the steering rack recovers.
if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
-
- # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
- # is inherently handled by scaling to STEER_MAX. The rack doesn't seem
- # to care about up/down rate, but we have some evidence it may do its
- # own rate limiting, and matching OP helps for accurate tuning.
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
-
- # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
- # a single frame with HCA disabled is an effective workaround.
if apply_steer == 0:
- # We can usually reset the timer for free, just by disabling HCA
- # when apply_steer is exactly zero, which happens by chance during
- # many steer torque direction changes. This could be expanded with
- # a small dead-zone to capture all zero crossings, but not seeing a
- # major need at this time.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
self.hcaEnabledFrameCount += 1
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s
- # The Kansas I-70 Crosswind Problem: if we truly do need to steer
- # in one direction for > 360 seconds, we have to disable HCA for a
- # frame while actively steering. Testing shows we can just set the
- # disabled flag, and keep sending non-zero torque, which keeps the
- # Panda torque rate limiting safety happy. Do so 3x within the 360
- # second window for safety and redundancy.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
hcaEnabled = True
- # FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds.
- # This is to detect the sending camera being stuck or frozen. OP
- # can trip this on a curve if steering is saturated. Avoid this by
- # reducing torque 0.01 Nm for one frame. Do so 3x within the 6
- # second period for safety and redundancy.
if self.apply_steer_last == apply_steer:
self.hcaSameTorqueCount += 1
if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s
@@ -88,9 +60,7 @@ class CarController():
self.hcaSameTorqueCount = 0
else:
self.hcaSameTorqueCount = 0
-
else:
- # Continue sending HCA_01 messages, with the enable flags turned off.
hcaEnabled = False
apply_steer = 0
@@ -99,23 +69,14 @@ class CarController():
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer,
idx, hcaEnabled))
- #--------------------------------------------------------------------------
- # #
- # Prepare LDW_02 HUD messages with lane borders, confidence levels, and #
- # the LKAS status LED. #
- # #
- #--------------------------------------------------------------------------
-
- # The factory camera emits this message at 10Hz. When OP is active, Panda
- # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
+ # **** HUD Controls ***************************************************** #
if frame % P.LDW_STEP == 0:
- if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired:
+ if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
else:
hud_alert = MQB_LDW_MESSAGES["none"]
-
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
CS.out.steeringPressed, hud_alert, left_lane_visible,
right_lane_visible, CS.ldw_lane_warning_left,
@@ -123,17 +84,9 @@ class CarController():
CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill,
left_lane_depart, right_lane_depart))
- #--------------------------------------------------------------------------
- # #
- # Prepare GRA_ACC_01 ACC control messages with button press events. #
- # #
- #--------------------------------------------------------------------------
+ # **** ACC Button Controls ********************************************** #
- # The car sends this message at 33hz. OP sends it on-demand only for
- # virtual button presses.
- #
- # First create any virtual button press event needed by openpilot, to sync
- # stock ACC with OP disengagement, or to auto-resume from stop.
+ # FIXME: this entire section is in desperate need of refactoring
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if not enabled and CS.out.cruiseState.enabled:
@@ -147,31 +100,6 @@ class CarController():
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["resumeCruise"] = True
- # OP/Panda can see this message but can't filter it when integrated at the
- # R242 LKAS camera. It could do so if integrated at the J533 gateway, but
- # we need a generalized solution that works for either. The message is
- # counter-protected, so we need to time our transmissions very precisely
- # to achieve fast and fault-free switching between message flows accepted
- # at the J428 ACC radar.
- #
- # Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP):
- #
- # CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6
- # EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^
- #
- # If OP needs to send a button press, it waits to see a GRA_ACC_01 message
- # counter change, and then immediately follows up with the next increment.
- # The OP message will be sent within about 1ms of the car's message, which
- # is about 2ms before the car's next message is expected. OP sends for an
- # arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new
- # message from the car.
- #
- # Because OP's counter is synced to the car, J428 immediately accepts the
- # OP messages as valid. Further messages from the car get discarded as
- # duplicates without a fault. When OP stops sending, the extra time gap
- # (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428
- # tolerates the gap just fine and control returns to the car immediately.
-
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
self.graMsgBusCounterPrev = CS.graMsgBusCounter
if self.graButtonStatesToSend is not None:
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 833cae6f7..ec3fc53e3 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -4,12 +4,12 @@ from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
-from selfdrive.car.volkswagen.values import DBC, CANBUS, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams
+from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
- can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
+ can_define = CANDefine(DBC_FILES.mqb)
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
elif CP.transmissionType == TransmissionType.direct:
@@ -133,8 +133,8 @@ class CarState(CarStateBase):
self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Setzen"])
self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Wiederaufnahme"])
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"])
- ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]["BH_Blinker_li"])
- ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]["BH_Blinker_re"])
+ ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"])
+ ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
# Read ACC hardware button type configuration info that has to pass thru
# to the radar. Ends up being different for steering wheel buttons vs
@@ -173,8 +173,8 @@ class CarState(CarStateBase):
("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left
("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right
("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open
- ("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on
- ("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on
+ ("Comfort_Signal_Left", "Blinkmodi_02", 0), # Left turn signal including comfort blink interval
+ ("Comfort_Signal_Right", "Blinkmodi_02", 0), # Right turn signal including comfort blink interval
("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed
@@ -216,6 +216,7 @@ class CarState(CarStateBase):
("Motor_14", 10), # From J623 Engine control module
("Airbag_02", 5), # From J234 Airbag control module
("Kombi_01", 2), # From J285 Instrument cluster
+ ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM
]
@@ -238,7 +239,7 @@ class CarState(CarStateBase):
signals += MqbExtraSignals.bsm_radar_signals
checks += MqbExtraSignals.bsm_radar_checks
- return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt)
+ return CANParser(DBC_FILES.mqb, signals, checks, CANBUS.pt)
@staticmethod
def get_cam_can_parser(CP):
@@ -257,7 +258,7 @@ class CarState(CarStateBase):
("LDW_02", 10) # From R242 Driver assistance camera
]
- return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
+ return CANParser(DBC_FILES.mqb, signals, checks, CANBUS.cam)
class MqbExtraSignals:
# Additional signal and message lists for optional or bus-portable controllers
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 55401af0f..1735a8dc7 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -1,5 +1,4 @@
from cereal import car
-from selfdrive.swaglog import cloudlog
from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, TransmissionType, GearShifter
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@@ -41,7 +40,6 @@ class CarInterface(CarInterfaceBase):
else:
# No trans message at all, must be a true stick-shift manual
ret.transmissionType = TransmissionType.manual
- cloudlog.info("Detected transmission type: %s", ret.transmissionType)
# Global tuning defaults, can be overridden per-vehicle
@@ -82,6 +80,11 @@ class CarInterface(CarInterfaceBase):
ret.mass = 1715 + STD_CARGO_KG
ret.wheelbase = 2.74
+ elif candidate == CAR.TOURAN_MK2:
+ # Average of SWB and LWB variants
+ ret.mass = 1516 + STD_CARGO_KG
+ ret.wheelbase = 2.79
+
elif candidate == CAR.AUDI_A3_MK3:
# Averages of all 8V A3 variants
ret.mass = 1335 + STD_CARGO_KG
@@ -124,7 +127,6 @@ class CarInterface(CarInterfaceBase):
ret.centerToFront = ret.wheelbase * 0.45
- ret.enableCamera = True
ret.enableBsm = 0x30F in fingerprint[0]
# TODO: get actual value, for now starting with reasonable value for
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index fcbf56ac1..3bad8fc3e 100644
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -1,7 +1,10 @@
# flake8: noqa
-from selfdrive.car import dbc_dict
+from collections import defaultdict
+from typing import Dict
+
from cereal import car
+from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
class CarControllerParams:
@@ -26,6 +29,11 @@ class CANBUS:
pt = 0
cam = 2
+class DBC_FILES:
+ mqb = "vw_mqb_2010" # Used for all cars with MQB-style CAN messaging
+
+DBC = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None)) # type: Dict[str, Dict[str, str]]
+
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
@@ -61,6 +69,7 @@ class CAR:
JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 Jetta
PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 Passat and variants
TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants
+ TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants
AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants
AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only)
SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
@@ -70,12 +79,22 @@ class CAR:
SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants
SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants
+# All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars
+# with a manual trans won't return transmission firmware, but all other cars will.
+#
+# The 0xF187 SW part number query should return in the form of N[NX][NX] NNN NNN [X[X]],
+# where N=number, X=letter, and the trailing two letters are optional. Performance
+# tuners sometimes tamper with that field (e.g. 8V0 9C0 BB0 1 from COBB/EQT). Tampered
+# ECU SW part numbers are invalid for vehicle ID and compatibility checks. Try to have
+# them repaired by the tuner before including them in openpilot.
+
FW_VERSIONS = {
CAR.ATLAS_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8703H906026AA\xf1\x899970',
b'\xf1\x8703H906026F \xf1\x896696',
b'\xf1\x8703H906026F \xf1\x899970',
+ b'\xf1\x8703H906026S \xf1\x896693',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158A \xf1\x893387',
@@ -84,6 +103,7 @@ FW_VERSIONS = {
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200',
b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900',
+ b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1',
@@ -104,6 +124,7 @@ FW_VERSIONS = {
b'\xf1\x8704E906024K \xf1\x896811',
b'\xf1\x8704E906027GR\xf1\x892394',
b'\xf1\x8704E906027MA\xf1\x894958',
+ b'\xf1\x8704L906026BP\xf1\x897608',
b'\xf1\x8704L906026NF\xf1\x899528',
b'\xf1\x8704L906056CR\xf1\x895813',
b'\xf1\x8704L906056HE\xf1\x893758',
@@ -123,6 +144,7 @@ FW_VERSIONS = {
b'\xf1\x878V0906264F \xf1\x890003',
b'\xf1\x878V0906264L \xf1\x890002',
b'\xf1\x878V0906264M \xf1\x890001',
+ b'\xf1\x878V09C0BB01 \xf1\x890001',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927749AP\xf1\x892943',
@@ -134,9 +156,11 @@ FW_VERSIONS = {
b'\xf1\x870CW300048J \xf1\x890611',
b'\xf1\x870D9300012 \xf1\x894913',
b'\xf1\x870D9300012 \xf1\x894937',
+ b'\xf1\x870D9300012 \xf1\x895045',
b'\xf1\x870D9300014M \xf1\x895004',
b'\xf1\x870D9300020S \xf1\x895201',
b'\xf1\x870D9300040S \xf1\x894311',
+ b'\xf1\x870D9300041H \xf1\x895220',
b'\xf1\x870DD300045K \xf1\x891120',
b'\xf1\x870DD300046F \xf1\x891601',
b'\xf1\x870GC300012A \xf1\x891403',
@@ -155,6 +179,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655AA\xf1\x890388\xf1\x82\0211413001113120053114317121C111C9113',
b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02314160011123300314211012230229333463100',
b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\023141600111233003142405A2252229333463100',
+ b'\xf1\x875Q0959655D \xf1\x890388\xf1\x82\0211413001113120006110417121A101A9113',
b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271112111312--071104171825102591131211',
b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271212111312--071104171838103891131211',
b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023341512112212--071104172328102891131211',
@@ -171,6 +196,7 @@ FW_VERSIONS = {
b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\00561A01612A0',
b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566A0J612A1',
b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A00514A1',
+ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A0J712A1',
b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\00571A0J714A1',
b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1',
b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\00571A01A18A1',
@@ -183,13 +209,15 @@ FW_VERSIONS = {
b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0',
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516A00604A1',
+ b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516A07A02A1',
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521A20B03A1',
b'\xf1\x875QD909144B \xf1\x891072\xf1\x82\x0521A00507A1',
b'\xf1\x875QM909144A \xf1\x891072\xf1\x82\x0521A20B03A1',
+ b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A00442A1',
+ b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\00571A01A16A1',
b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\00571A01A18A1',
b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A17A1',
- b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\00521A00442A1',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\00101',
@@ -267,21 +295,47 @@ FW_VERSIONS = {
},
CAR.TIGUAN_MK2: {
(Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704L906026EJ\xf1\x893661',
+ b'\xf1\x8704L906027G \xf1\x899893',
b'\xf1\x8783A907115B \xf1\x890005',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158DT\xf1\x893698',
+ b'\xf1\x870DL300011N \xf1\x892001',
+ b'\xf1\x870DL300013A \xf1\x893005',
],
(Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100',
b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100',
+ b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100',
],
(Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603',
+ b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\00571A60634A1',
b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1',
],
(Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x872Q0907572J \xf1\x890156',
b'\xf1\x872Q0907572R \xf1\x890372',
],
},
+ CAR.TOURAN_MK2: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x8704L906026HM\xf1\x893017',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x870CW300041E \xf1\x891005',
+ ],
+ (Ecu.srs, 0x715, None): [
+ b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\023363500213533353141324C4732479333313100',
+ ],
+ (Ecu.eps, 0x712, None): [
+ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105',
+ ],
+ (Ecu.fwdRadar, 0x757, None): [
+ b'\xf1\x873Q0907572C \xf1\x890195',
+ ],
+ },
CAR.AUDI_A3_MK3: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906023AN\xf1\x893695',
@@ -412,21 +466,26 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906027HD\xf1\x893742',
b'\xf1\x8704L906021DT\xf1\x898127',
+ b'\xf1\x8704L906026BS\xf1\x891541',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x870CW300043B \xf1\x891601',
+ b'\xf1\x870D9300041J \xf1\x894902',
b'\xf1\x870D9300041P \xf1\x894507',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655AC\xf1\x890200\xf1\x82\r11120011100010022212110200',
+ b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200',
b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100',
],
(Ecu.eps, 0x712, None): [
+ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A01513A1',
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101',
+ b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
b'\xf1\x875Q0907572P \xf1\x890682',
],
},
@@ -474,19 +533,3 @@ FW_VERSIONS = {
],
},
}
-
-DBC = {
- CAR.ATLAS_MK1: dbc_dict('vw_mqb_2010', None),
- CAR.GOLF_MK7: dbc_dict('vw_mqb_2010', None),
- CAR.JETTA_MK7: dbc_dict('vw_mqb_2010', None),
- CAR.PASSAT_MK8: dbc_dict('vw_mqb_2010', None),
- CAR.TIGUAN_MK2: dbc_dict('vw_mqb_2010', None),
- CAR.AUDI_A3_MK3: dbc_dict('vw_mqb_2010', None),
- CAR.AUDI_Q2_MK1: dbc_dict('vw_mqb_2010', None),
- CAR.SEAT_ATECA_MK1: dbc_dict('vw_mqb_2010', None),
- CAR.SEAT_LEON_MK3: dbc_dict('vw_mqb_2010', None),
- CAR.SKODA_KODIAQ_MK1: dbc_dict('vw_mqb_2010', None),
- CAR.SKODA_OCTAVIA_MK3: dbc_dict('vw_mqb_2010', None),
- CAR.SKODA_SCALA_MK1: dbc_dict('vw_mqb_2010', None),
- CAR.SKODA_SUPERB_MK3: dbc_dict('vw_mqb_2010', None),
-}
diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc
index c6ad4061a..951a92467 100644
--- a/selfdrive/clocksd/clocksd.cc
+++ b/selfdrive/clocksd/clocksd.cc
@@ -1,9 +1,10 @@
-#include
-#include
#include
#include
#include
+#include
+#include
+
// Apple doesn't have timerfd
#ifdef __APPLE__
#include
@@ -52,7 +53,7 @@ int main() {
if (err < 0) break;
#else
// Just run at 1Hz on apple
- while (!do_exit){
+ while (!do_exit) {
util::sleep_for(1000);
#endif
diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript
index 8f6c1dc18..57c789479 100644
--- a/selfdrive/common/SConscript
+++ b/selfdrive/common/SConscript
@@ -35,3 +35,6 @@ else:
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
Export('_common', '_gpucommon', '_gpu_libs')
+
+if GetOption('test'):
+ env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc
index 526c66b22..68207c286 100644
--- a/selfdrive/common/clutil.cc
+++ b/selfdrive/common/clutil.cc
@@ -1,10 +1,9 @@
#include "selfdrive/common/clutil.h"
-#include
-#include
-#include
#include
+#include
+#include
#include
#include
#include
diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h
index 71523c273..4c8723d64 100644
--- a/selfdrive/common/clutil.h
+++ b/selfdrive/common/clutil.h
@@ -1,8 +1,7 @@
#pragma once
-#include
-#include
-#include
+#include
+#include
#ifdef __APPLE__
#include
diff --git a/selfdrive/common/glutil.cc b/selfdrive/common/glutil.cc
index 7e53975e1..f13e132a8 100644
--- a/selfdrive/common/glutil.cc
+++ b/selfdrive/common/glutil.cc
@@ -1,9 +1,8 @@
#include "selfdrive/common/glutil.h"
-#include
-#include
-#include
-
+#include
+#include
+#include
#include
static GLuint load_shader(GLenum shaderType, const char *src) {
diff --git a/selfdrive/common/gpio.cc b/selfdrive/common/gpio.cc
index 8a2885316..033d6da4f 100644
--- a/selfdrive/common/gpio.cc
+++ b/selfdrive/common/gpio.cc
@@ -1,30 +1,31 @@
#include "selfdrive/common/gpio.h"
#include
-#include
#include
+#include
+
#include "selfdrive/common/util.h"
// We assume that all pins have already been exported on boot,
// and that we have permission to write to them.
-int gpio_init(int pin_nr, bool output){
+int gpio_init(int pin_nr, bool output) {
char pin_dir_path[50];
int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path),
"/sys/class/gpio/gpio%d/direction", pin_nr);
- if(pin_dir_path_len <= 0){
+ if(pin_dir_path_len <= 0) {
return -1;
}
const char *value = output ? "out" : "in";
return util::write_file(pin_dir_path, (void*)value, strlen(value));
}
-int gpio_set(int pin_nr, bool high){
+int gpio_set(int pin_nr, bool high) {
char pin_val_path[50];
int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path),
"/sys/class/gpio/gpio%d/value", pin_nr);
- if(pin_val_path_len <= 0){
+ if(pin_val_path_len <= 0) {
return -1;
}
return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1);
diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc
index c2842948b..80738be80 100644
--- a/selfdrive/common/i2c.cc
+++ b/selfdrive/common/i2c.cc
@@ -1,11 +1,11 @@
#include "selfdrive/common/i2c.h"
#include
-#include
#include
#include
#include
+#include
#include
#include "selfdrive/common/swaglog.h"
@@ -19,21 +19,21 @@ extern "C" {
#include
}
-I2CBus::I2CBus(uint8_t bus_id){
+I2CBus::I2CBus(uint8_t bus_id) {
char bus_name[20];
snprintf(bus_name, 20, "/dev/i2c-%d", bus_id);
i2c_fd = open(bus_name, O_RDWR);
- if(i2c_fd < 0){
+ if(i2c_fd < 0) {
throw std::runtime_error("Failed to open I2C bus");
}
}
-I2CBus::~I2CBus(){
- if(i2c_fd >= 0){ close(i2c_fd); }
+I2CBus::~I2CBus() {
+ if(i2c_fd >= 0) { close(i2c_fd); }
}
-int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){
+int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) {
int ret = 0;
ret = ioctl(i2c_fd, I2C_SLAVE, device_address);
@@ -46,7 +46,7 @@ fail:
return ret;
}
-int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){
+int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) {
int ret = 0;
ret = ioctl(i2c_fd, I2C_SLAVE, device_address);
@@ -61,14 +61,14 @@ fail:
#else
-I2CBus::I2CBus(uint8_t bus_id){
+I2CBus::I2CBus(uint8_t bus_id) {
UNUSED(bus_id);
i2c_fd = -1;
}
-I2CBus::~I2CBus(){}
+I2CBus::~I2CBus() {}
-int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){
+int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) {
UNUSED(device_address);
UNUSED(register_address);
UNUSED(buffer);
@@ -76,7 +76,7 @@ int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t
return -1;
}
-int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){
+int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) {
UNUSED(device_address);
UNUSED(register_address);
UNUSED(data);
diff --git a/selfdrive/common/i2c.h b/selfdrive/common/i2c.h
index d5788510d..0669116bb 100644
--- a/selfdrive/common/i2c.h
+++ b/selfdrive/common/i2c.h
@@ -1,6 +1,7 @@
#pragma once
-#include
+#include
+
#include
class I2CBus {
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 83aff9705..4eb306805 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,5 +1,7 @@
#pragma once
const int TRAJECTORY_SIZE = 33;
+const int LON_MPC_N = 32;
+const int LAT_MPC_N = 16;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 45eac661b..a14076513 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -5,14 +5,14 @@
#endif // _GNU_SOURCE
#include
-#include
-#include
-#include
#include
#include
#include
#include
+#include
+#include
+#include
#include
#include
@@ -42,9 +42,9 @@ void params_sig_handler(int signal) {
params_do_exit = 1;
}
-int fsync_dir(const char* path){
+int fsync_dir(const char* path) {
int fd = HANDLE_EINTR(open(path, O_RDONLY, 0755));
- if (fd < 0){
+ if (fd < 0) {
return -1;
}
@@ -78,7 +78,7 @@ int mkdir_p(std::string path) {
return 0;
}
-bool ensure_params_path(const std::string ¶m_path, const std::string &key_path) {
+static bool create_params_path(const std::string ¶m_path, const std::string &key_path) {
// Make sure params path exists
if (!util::file_exists(param_path) && mkdir_p(param_path) != 0) {
return false;
@@ -117,6 +117,12 @@ bool ensure_params_path(const std::string ¶m_path, const std::string &key_pa
return chmod(key_path.c_str(), 0777) == 0;
}
+static void ensure_params_path(const std::string ¶ms_path) {
+ if (!create_params_path(params_path, params_path + "/d")) {
+ throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
+ }
+}
+
class FileLock {
public:
FileLock(const std::string& file_name, int op) : fn_(file_name), op_(op) {}
@@ -128,7 +134,6 @@ class FileLock {
return;
}
if (HANDLE_EINTR(flock(fd_, op_)) < 0) {
- close(fd_);
LOGE("Failed to lock file %s, errno=%d", fn_.c_str(), errno);
}
}
@@ -145,6 +150,7 @@ std::unordered_map keys = {
{"ApiCache_DriveStats", PERSISTENT},
{"ApiCache_Device", PERSISTENT},
{"ApiCache_Owner", PERSISTENT},
+ {"ApiCache_NavDestinations", PERSISTENT},
{"AthenadPid", PERSISTENT},
{"CalibrationParams", PERSISTENT},
{"CarBatteryCapacity", PERSISTENT},
@@ -153,7 +159,8 @@ std::unordered_map keys = {
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
{"CommunityFeaturesToggle", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
- {"EnableLteOnroad", PERSISTENT},
+ {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
+ {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
{"EndToEndToggle", PERSISTENT},
{"CompletedTrainingVersion", PERSISTENT},
{"DisablePowerDown", PERSISTENT},
@@ -186,6 +193,7 @@ std::unordered_map keys = {
{"LiveParameters", PERSISTENT},
{"MapboxToken", PERSISTENT},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
+ {"NavSettingTime24h", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
@@ -217,15 +225,19 @@ std::unordered_map keys = {
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
{"Offroad_NvmeMissing", CLEAR_ON_MANAGER_START},
{"ForcePowerDown", CLEAR_ON_MANAGER_START},
+ {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
};
} // namespace
Params::Params(bool persistent_param) : Params(persistent_param ? persistent_params_path : default_params_path) {}
+std::once_flag default_params_path_ensured;
Params::Params(const std::string &path) : params_path(path) {
- if (!ensure_params_path(params_path, params_path + "/d")) {
- throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
+ if (path == default_params_path) {
+ std::call_once(default_params_path_ensured, ensure_params_path, path);
+ } else {
+ ensure_params_path(path);
}
}
@@ -268,7 +280,7 @@ int Params::put(const char* key, const char* value, size_t value_size) {
// fsync parent directory
path = params_path + "/d";
result = fsync_dir(path.c_str());
- } while(0);
+ } while (false);
close(tmp_fd);
remove(tmp_path.c_str());
diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h
index 3d59f07b1..bc1372295 100644
--- a/selfdrive/common/params.h
+++ b/selfdrive/common/params.h
@@ -1,7 +1,5 @@
#pragma once
-#include
-
#include