Merge pull request #11 from eFiniLan/2023-10-17

vehicle support as of 2023.10.17

Author: eFiniLan

common/kalman/SConscript

@@ -1,3 +1,5 @@
 Import('envCython')
 
-envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')
+simple_kalman_python = envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')
+
+Export('simple_kalman_python')

common/kalman/simple_kalman.py

@@ -1,3 +1,12 @@
-# pylint: skip-file
 from openpilot.common.kalman.simple_kalman_impl import KF1D as KF1D
 assert KF1D
+import numpy as np
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K

launch_chffrplus.sh

@@ -165,6 +165,12 @@ function two_init {
     echo "Installing spidev.cpython-38.so..."
     cp -f "$LIB_PATH/spidev.cpython-38.so" "$PY_LIB_DEST/"
   fi
+  # StrEnum in values.py
+  MODULE="strenum"
+  if [ ! -d "$PY_LIB_DEST/$MODULE" ]; then
+    echo "Installing $MODULE..."
+    tar -zxvf "$LIB_PATH/$MODULE.tar.gz" -C "$PY_LIB_DEST/"
+  fi
   mount -o remount,r /system
 
   # Check for NEOS update

panda

@@ -10,7 +10,7 @@
 
 # Supported Cars
 
-A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
+A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
 
 # {{all_car_info | length}} Supported Cars
 

selfdrive/car/CARS_template.md

@@ -1,6 +1,6 @@
 # functions common among cars
 from collections import namedtuple
-from typing import Dict, Optional
+from typing import Dict, List, Optional
 
 import capnp
 
@@ -24,20 +24,23 @@ def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
   return val_steady
 
 
-def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
-                        unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder:
-  if cur_but != unpressed:
-    be = car.CarState.ButtonEvent(pressed=True)
-    but = cur_but
-  else:
-    be = car.CarState.ButtonEvent(pressed=False)
-    but = prev_but
-  be.type = buttons_dict.get(but, ButtonType.unknown)
-  return be
+def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
+                         unpressed_btn: int = 0) -> List[capnp.lib.capnp._DynamicStructBuilder]:
+  events: List[capnp.lib.capnp._DynamicStructBuilder] = []
+
+  if cur_btn == prev_btn:
+    return events
+
+  # Add events for button presses, multiple when a button switches without going to unpressed
+  for pressed, btn in ((False, prev_btn), (True, cur_btn)):
+    if btn != unpressed_btn:
+      events.append(car.CarState.ButtonEvent(pressed=pressed,
+                                             type=buttons_dict.get(btn, ButtonType.unknown)))
+  return events
 
 
 def gen_empty_fingerprint():
-  return {i: {} for i in range(0, 8)}
+  return {i: {} for i in range(8)}
 
 
 # these params were derived for the Civic and used to calculate params for other cars
@@ -232,4 +235,4 @@ def update(self, current_value, current_counter):
     self.previous_counter = current_counter
     self.previous_value = current_value
 
-    return self.rate
+    return self.rate

selfdrive/car/__init__.py

@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 import math
 from cereal import car
 from openpilot.common.realtime import DT_CTRL

selfdrive/car/body/interface.py

@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):

selfdrive/car/body/radar_interface.py

@@ -1,3 +1,4 @@
+from strenum import StrEnum
 from typing import Dict
 
 from cereal import car
@@ -21,7 +22,7 @@ def __init__(self, CP):
     pass
 
 
-class CAR:
+class CAR(StrEnum):
   BODY = "COMMA BODY"
 
 

selfdrive/car/body/values.py

@@ -1,4 +1,5 @@
 import os
+import time
 from typing import Callable, Dict, List, Optional, Tuple
 
 from cereal import car
@@ -65,9 +66,8 @@ def load_interfaces(brand_names):
 def _get_interface_names() -> Dict[str, List[str]]:
   # returns a dict of brand name and its respective models
   brand_names = {}
-  for brand_name, model_names in get_interface_attr("CAR").items():
-    model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
-    brand_names[brand_name] = model_names
+  for brand_name, brand_models in get_interface_attr("CAR").items():
+    brand_names[brand_name] = [model.value for model in brand_models]
 
   return brand_names
 
@@ -125,16 +125,16 @@ def fingerprint(logcan, sendcan, num_pandas):
   ecu_rx_addrs = set()
   params = Params()
 
+  start_time = time.monotonic()
   if not skip_fw_query:
     # Vin query only reliably works through OBDII
     bus = 1
 
     cached_params = params.get("CarParamsCache")
     if cached_params is not None:
-      cached_params = car.CarParams.from_bytes(cached_params)
-      # with car.CarParams.from_bytes(cached_params) as cached_params:
-      if cached_params.carName == "mock":
-        cached_params = None
+      with car.CarParams.from_bytes(cached_params) as cached_params:
+        if cached_params.carName == "mock":
+          cached_params = None
 
     if cached_params is not None and len(cached_params.carFw) > 0 and \
        cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
@@ -166,6 +166,8 @@ def fingerprint(logcan, sendcan, num_pandas):
   set_obd_multiplexing(params, False)
   params.put_bool("FirmwareQueryDone", True)
 
+  fw_query_time = time.monotonic() - start_time
+
   # CAN fingerprint
   # drain CAN socket so we get the latest messages
   messaging.drain_sock_raw(logcan)
@@ -186,7 +188,7 @@ def fingerprint(logcan, sendcan, num_pandas):
 
   cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
                  fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, fingerprints=finger,
-                 error=True)
+                 fw_query_time=fw_query_time, error=True)
   return car_fingerprint, finger, vin, car_fw, source, exact_match
 
 

selfdrive/car/car_helpers.py

@@ -1,7 +1,7 @@
 from opendbc.can.packer import CANPacker
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits
-from openpilot.selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons
+from openpilot.selfdrive.car.chrysler import chryslercan
 from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
 
 
@@ -31,17 +31,18 @@ def update(self, CC, CS, now_nanos):
       # ACC cancellation
       if CC.cruiseControl.cancel:
         self.last_button_frame = self.frame
-        can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
 
       # ACC resume from standstill
       elif CC.cruiseControl.resume:
         self.last_button_frame = self.frame
-        can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
 
     # HUD alerts
     if self.frame % 25 == 0:
       if CS.lkas_car_model != -1:
-        can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
+        can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
+                                                     self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
         self.hud_count += 1
 
     # steering
@@ -72,7 +73,7 @@ def update(self, CC, CS, now_nanos):
         apply_steer = 0
       self.apply_steer_last = apply_steer
 
-      can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
+      can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
 
     self.frame += 1
 

selfdrive/car/chrysler/carcontroller.py

@@ -1,5 +1,6 @@
 # ruff: noqa: E501
 from enum import IntFlag
+from strenum import StrEnum
 from dataclasses import dataclass, field
 from typing import Dict, List, Optional, Union
 
@@ -16,7 +17,7 @@ class ChryslerFlags(IntFlag):
   HIGHER_MIN_STEERING_SPEED = 1
 
 
-class CAR:
+class CAR(StrEnum):
   # Chrysler
   PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
   PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"

selfdrive/car/chrysler/values.py

@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
 from openpilot.system.swaglog import cloudlog
 

selfdrive/car/disable_ecu.py

@@ -85,6 +85,7 @@ class CarHarness(EnumBase):
   bosch_a = BaseCarHarness("Honda Bosch A connector")
   bosch_b = BaseCarHarness("Honda Bosch B connector")
   toyota_a = BaseCarHarness("Toyota A connector")
+  toyota_b = BaseCarHarness("Toyota B connector")
   subaru_a = BaseCarHarness("Subaru A connector")
   subaru_b = BaseCarHarness("Subaru B connector")
   subaru_c = BaseCarHarness("Subaru C connector")
@@ -110,6 +111,7 @@ class CarHarness(EnumBase):
   hyundai_o = BaseCarHarness("Hyundai O connector")
   hyundai_p = BaseCarHarness("Hyundai P connector")
   hyundai_q = BaseCarHarness("Hyundai Q connector")
+  hyundai_r = BaseCarHarness("Hyundai R connector")
   custom = BaseCarHarness("Developer connector")
   obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable, Cable.long_obdc_cable], has_connector=False)
   gm = BaseCarHarness("GM connector")
@@ -121,9 +123,9 @@ class CarHarness(EnumBase):
 
 
 class Device(EnumBase):
-  three = BasePart("comma three", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
-  # variant of comma three with angled mounts
-  three_angled_mount = BasePart("comma three", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
+  threex = BasePart("comma 3X", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
+  # variant of comma 3X with angled mounts
+  threex_angled_mount = BasePart("comma 3X", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
   red_panda = BasePart("red panda")
 
 
@@ -147,7 +149,7 @@ class PartType(Enum):
   tool = Tool
 
 
-DEFAULT_CAR_PARTS: List[EnumBase] = [Device.three]
+DEFAULT_CAR_PARTS: List[EnumBase] = [Device.threex]
 
 
 @dataclass
@@ -158,7 +160,7 @@ def __call__(self):
     return copy.deepcopy(self)
 
   @classmethod
-  def common(cls, add: List[EnumBase] = None, remove: List[EnumBase] = None):
+  def common(cls, add: Optional[List[EnumBase]] = None, remove: Optional[List[EnumBase]] = None):
     p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])]
     return cls(p)
 
@@ -174,8 +176,8 @@ def all_parts(self):
 
 class CommonFootnote(Enum):
   EXP_LONG_AVAIL = CarFootnote(
-    "Experimental openpilot longitudinal control is available behind a toggle; " +
-    "the toggle is only available in non-release branches such as `devel` or `master-ci`. ",
+    "openpilot Longitudinal Control (Alpha) is available behind a toggle; " +
+    "the toggle is only available in non-release branches such as `devel` or `master-ci`.",
     Column.LONGITUDINAL, docs_only=True)
   EXP_LONG_DSU = CarFootnote(
     "By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. " +

selfdrive/car/docs_definitions.py

@@ -2,8 +2,7 @@
 from openpilot.common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
-from openpilot.selfdrive.car.ford.fordcan import CanBus, create_acc_msg, create_acc_ui_msg, create_button_msg, \
-                                       create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg
+from openpilot.selfdrive.car.ford import fordcan
 from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -27,7 +26,7 @@ def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.VM = VM
     self.packer = CANPacker(dbc_name)
-    self.CAN = CanBus(CP)
+    self.CAN = fordcan.CanBus(CP)
     self.frame = 0
 
     self.apply_curvature_last = 0
@@ -47,15 +46,15 @@ def update(self, CC, CS, now_nanos):
 
     ### acc buttons ###
     if CC.cruiseControl.cancel:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
-      can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
     elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
-      can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
     # if stock lane centering isn't off, send a button press to toggle it off
     # the stock system checks for steering pressed, and eventually disengages cruise control
     elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
 
     ### lateral control ###
     # send steer msg at 20Hz
@@ -73,13 +72,13 @@ def update(self, CC, CS, now_nanos):
         # TODO: extended mode
         mode = 1 if CC.latActive else 0
         counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF
-        can_sends.append(create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
+        can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
       else:
-        can_sends.append(create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
+        can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
 
     # send lka msg at 33Hz
     if (self.frame % CarControllerParams.LKA_STEP) == 0:
-      can_sends.append(create_lka_msg(self.packer, self.CAN))
+      can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
 
     ### longitudinal control ###
     # send acc msg at 50Hz
@@ -91,16 +90,16 @@ def update(self, CC, CS, now_nanos):
         gas = CarControllerParams.INACTIVE_GAS
 
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
-      can_sends.append(create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping))
+      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
     # send lkas ui msg at 1Hz or if ui state changes
     if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
-      can_sends.append(create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
+      can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
     # send acc ui msg at 5Hz or if ui state changes
     if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
-      can_sends.append(create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
+      can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
                                          fcw_alert, CS.out.cruiseState.standstill, hud_control,
                                          CS.acc_tja_status_stock_values))
 

selfdrive/car/ford/carcontroller.py

@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 from cereal import car
 from panda import Panda
 from openpilot.common.conversions import Conversions as CV

selfdrive/car/ford/interface.py

@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 from math import cos, sin
 from cereal import car
 from opendbc.can.parser import CANParser

selfdrive/car/ford/radar_interface.py

@@ -6,7 +6,7 @@
 import capnp
 
 from cereal import car
-from selfdrive.car.ford.values import FW_QUERY_CONFIG, FW_VERSIONS
+from openpilot.selfdrive.car.ford.values import FW_QUERY_CONFIG, FW_VERSIONS
 
 Ecu = car.CarParams.Ecu