Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

selfdrive/car: ban cereal and capnp #33208

Merged
merged 142 commits into from
Aug 16, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
142 commits
Select commit Hold shift + click to select a range
1d3e059
ban cereal and msgq
sshane Aug 6, 2024
c17e80f
common too
sshane Aug 6, 2024
c2ec763
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 8, 2024
db592ed
do toyota/values.py
sshane Aug 8, 2024
d2c5602
do all fingerprints
sshane Aug 8, 2024
1b7dca8
example without builder
sshane Aug 9, 2024
3597057
this still works, but no type checking anymore
sshane Aug 9, 2024
2e2f8f5
stash
sshane Aug 9, 2024
7795697
wtf, how does this work
sshane Aug 9, 2024
9ef0fab
okay actually not bad
sshane Aug 9, 2024
37d40a3
safe
sshane Aug 9, 2024
965f692
epic!
sshane Aug 9, 2024
bc24202
stash data_structures.py
sshane Aug 9, 2024
95351b1
some clean up
sshane Aug 9, 2024
a06264a
hell yeah
sshane Aug 9, 2024
e21593e
clean up old file
sshane Aug 9, 2024
90239b7
add to delete
sshane Aug 9, 2024
78139f7
delete
sshane Aug 9, 2024
e54ad02
switch more CarParams stuff over
sshane Aug 9, 2024
05d9366
fix car tests by removing cereal! mypy forgets about dataclass if we …
sshane Aug 9, 2024
94ef0f8
fix this too
sshane Aug 9, 2024
593b0a5
fix this too
sshane Aug 9, 2024
72895af
remove more cereal and add some good hyundai tests
sshane Aug 9, 2024
a55b8d2
bunch more typing
sshane Aug 9, 2024
b889d73
override default with 20hz radar
sshane Aug 9, 2024
419c143
temp capnp converter helper
sshane Aug 9, 2024
d40da13
more lateralTuning
sshane Aug 9, 2024
6e6ab9f
small union replicator is better than what i was trying, and fixes my…
sshane Aug 9, 2024
ce290a0
can keep all this the same now!
sshane Aug 9, 2024
e8afc6a
type ret: CarParams, add more missing structs, revert lateralTuning c…
sshane Aug 9, 2024
e52fae0
revert more
sshane Aug 9, 2024
bb28b22
get first enum automatically, but ofc mypy doesn't pick up the new me…
sshane Aug 9, 2024
672fc00
Revert "get first enum automatically, but ofc mypy doesn't pick up th…
sshane Aug 9, 2024
37304e1
remove cereal from car_helpers (TODO: caching)
sshane Aug 9, 2024
9b72521
remove a bunch of temp lines
sshane Aug 9, 2024
418d1ce
use dataclass_transform!
sshane Aug 9, 2024
b926893
remove some car.CarParams from the interfaces
sshane Aug 10, 2024
bddb9f3
remove rest of car.CarParams from the interfaces
sshane Aug 10, 2024
25fa73a
same which() API
sshane Aug 10, 2024
6f16c66
sort
sshane Aug 10, 2024
2b164dd
from cereal/cache from fingerprinting!
sshane Aug 10, 2024
20b34cb
more typing
sshane Aug 10, 2024
594d0b7
dataclass to capnp helper for CarParams, cached it since it's kinda slow
sshane Aug 10, 2024
abbb97a
(partial) fix process replay fingerprintig for new API
sshane Aug 10, 2024
0c89b2b
latcontrollers take capnp
sshane Aug 10, 2024
d17f3d2
forgot this
sshane Aug 10, 2024
06a879d
fix test_models
sshane Aug 10, 2024
81c1c05
fix unit tests
sshane Aug 10, 2024
b8a8b43
not here
sshane Aug 10, 2024
d4700d1
VehicleModel and controller still takes capnp CP since they get it fr…
sshane Aug 10, 2024
bc4954f
fix modeld test
sshane Aug 10, 2024
6f9ed71
more fix
sshane Aug 10, 2024
4359fa5
need to namespace to structs, since CarState is both class and struct
sshane Aug 10, 2024
ede08d4
this was never in the base class?!
sshane Aug 10, 2024
f615d11
clean that up again
sshane Aug 10, 2024
a6c4a32
fix import error
sshane Aug 10, 2024
e5efda9
cmts and more structs
sshane Aug 10, 2024
07bc45c
remove some more cereal from toyota + convert CarState to capnp
sshane Aug 10, 2024
df954ed
bruh this was wrong
sshane Aug 10, 2024
3a4ce84
replace more cereal
sshane Aug 10, 2024
48c0b73
EventName is one of the last things...
sshane Aug 10, 2024
eb52d70
replace a bunch more cereal.car
sshane Aug 10, 2024
c8b7d1c
missing imports
sshane Aug 10, 2024
83cb089
more
sshane Aug 10, 2024
daf3ff7
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 10, 2024
5297c85
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 10, 2024
777f629
can fix this typing now
sshane Aug 10, 2024
12d89d0
proper toyota+others CS typing!
sshane Aug 10, 2024
2c7baf3
mypy can detect return type of CS.update() now
sshane Aug 10, 2024
61b2446
fix redeclaration of cruise_buttons type
sshane Aug 10, 2024
7628608
mypy is only complaining about events now
sshane Aug 10, 2024
b7367c3
temp fix
sshane Aug 11, 2024
7bc96d1
add carControl struct
sshane Aug 11, 2024
037d1db
replace CarControl
sshane Aug 11, 2024
397a555
fine now
sshane Aug 11, 2024
36610e5
lol this was wrong too
sshane Aug 11, 2024
b51dd22
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 12, 2024
5ac1f7b
fix crash
sshane Aug 13, 2024
a851b82
include my failed attempts at recursively converting to dataclass (do…
sshane Aug 13, 2024
09bb822
clean up
sshane Aug 13, 2024
ff2434f
try out attr.s for its converter (doesn't work recursively yet, but i…
sshane Aug 13, 2024
e5bed3c
Revert "try out attr.s for its converter (doesn't work recursively ye…
sshane Aug 13, 2024
5873989
test processes doesn't fail anymore (on toyota)!
sshane Aug 13, 2024
cfb242b
fix honda crash
sshane Aug 13, 2024
c1762af
stash
sshane Aug 13, 2024
7558a02
Revert "stash"
sshane Aug 14, 2024
e0b9e45
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 14, 2024
6249420
remove a bunch more cereal!
sshane Aug 14, 2024
dff4aaa
LET'S GOOO
sshane Aug 14, 2024
134f523
fix these tests
sshane Aug 14, 2024
989d898
and these
sshane Aug 14, 2024
391445e
and that
sshane Aug 14, 2024
39cf327
stash, something is wrong with hyundai enable
sshane Aug 14, 2024
19d77b6
Revert "stash, something is wrong with hyundai enable"
sshane Aug 15, 2024
cfb8453
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 15, 2024
39982ae
forgot these
sshane Aug 15, 2024
232b37c
remove cereal from fw_versions
sshane Aug 15, 2024
23d041e
Revert "remove cereal from fw_versions"
sshane Aug 15, 2024
cf62b18
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 15, 2024
2bd260d
remove rest of the cereal exceptions!
sshane Aug 15, 2024
e97d4fa
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 15, 2024
26c02c7
fix that
sshane Aug 15, 2024
377facf
add typing to radard since I didn't realize RI.update() switched from…
sshane Aug 15, 2024
610ee98
and here too!
sshane Aug 15, 2024
d98179d
add TODO for slots
sshane Aug 15, 2024
71b5073
needed CS to be capnp, fix comparisons, and type hint car_specific so…
sshane Aug 15, 2024
b4c4da6
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 15, 2024
c547a96
remove the struct converter
sshane Aug 15, 2024
1a61723
save ~4-5% CPU at 100hz, we don't modify after so no need to deepcopy
sshane Aug 15, 2024
6f307cf
deepcopy -> copy: we can technically make a reference, but copy is al…
sshane Aug 15, 2024
d398f38
add non-copying asdict function
sshane Aug 16, 2024
a292276
should save ~3% CPU (still 4% above baseline)
sshane Aug 16, 2024
6d56b1a
fix that, no dict support
sshane Aug 16, 2024
7ad5227
~27% decrease in time for 20k iterations on 3X (3.37857 -> 2.4821s)
sshane Aug 16, 2024
b134771
give a better name
sshane Aug 16, 2024
f2b8897
fix
sshane Aug 16, 2024
a0ce77f
dont support none, capitalize
sshane Aug 16, 2024
f316334
sheesh, this called type() on every field
sshane Aug 16, 2024
c113ea3
remove CS.events, clean up
sshane Aug 16, 2024
db3914b
bump card %
sshane Aug 16, 2024
29e6ecb
this was a bug on master!
sshane Aug 16, 2024
297f5ab
add a which enum
sshane Aug 16, 2024
8cdb765
default to pid
sshane Aug 16, 2024
fa4965a
revert
sshane Aug 16, 2024
1840d54
update refs
sshane Aug 16, 2024
bd9fe5e
not needed, but consistent
sshane Aug 16, 2024
e3dcaf3
just Ecu
sshane Aug 16, 2024
b1f7227
don't need to do this in this pr
sshane Aug 16, 2024
a99f78d
clean up
sshane Aug 16, 2024
07531cc
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 16, 2024
f4d61c7
no cast
sshane Aug 16, 2024
6d7e428
consistent typing
sshane Aug 16, 2024
95c10fe
rm
sshane Aug 16, 2024
e94b7c1
fix
sshane Aug 16, 2024
a5e363d
Merge remote-tracking branch 'upstream/master' into no-capnp-cars-simple
sshane Aug 16, 2024
18e11ac
can do this if we're desperate for the last few %
sshane Aug 16, 2024
0c11c6c
Revert "can do this if we're desperate for the last few %"
sshane Aug 16, 2024
d134be7
type this
sshane Aug 16, 2024
2f3347e
don't need to convert carControl
sshane Aug 16, 2024
83b9a65
i guess don't support set either
sshane Aug 16, 2024
92b8083
fix CP type hint
sshane Aug 16, 2024
efe78e2
simplify that
sshane Aug 16, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions .importlinter
Original file line number Diff line number Diff line change
@@ -1,13 +1,17 @@
[importlinter]
root_packages =
openpilot
cereal
capnp

[importlinter:contract:1]
name = Forbid imports from openpilot.selfdrive.car to openpilot.system
type = forbidden
source_modules =
openpilot.selfdrive.car
forbidden_modules =
cereal
capnp
openpilot.common
openpilot.selfdrive.controls
openpilot.selfdrive.debug
Expand Down Expand Up @@ -44,5 +48,16 @@ ignore_imports =
openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.pandad
openpilot.selfdrive.car.tests.test_models -> openpilot.selfdrive.pandad
openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.test.fuzzy_generation
openpilot.selfdrive.car.tests.test_models -> capnp
openpilot.selfdrive.car.tests.test_car_interfaces -> cereal
openpilot.selfdrive.car.tests.test_car_interfaces -> cereal.messaging
openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.test.fuzzy_generation
openpilot.selfdrive.car.tests.test_models -> cereal
openpilot.selfdrive.car.tests.test_models -> cereal.messaging
openpilot.selfdrive.car.card -> cereal
openpilot.selfdrive.car.card -> cereal.messaging
openpilot.selfdrive.car.car_specific -> openpilot.selfdrive.controls.lib.events
openpilot.selfdrive.car.car_specific -> cereal
openpilot.selfdrive.car.car_specific -> cereal.messaging
openpilot.selfdrive.car.card -> capnp
unmatched_ignore_imports_alerting = warn
22 changes: 10 additions & 12 deletions selfdrive/car/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,8 @@
from enum import IntFlag, ReprEnum, EnumType
from dataclasses import replace

import capnp

from cereal import car
from panda.python.uds import SERVICE_TYPE
from openpilot.selfdrive.car import structs
from openpilot.selfdrive.car.can_definitions import CanData
from openpilot.selfdrive.car.docs_definitions import CarDocs
from openpilot.selfdrive.car.common.numpy_fast import clip, interp
Expand All @@ -23,7 +21,7 @@
# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.

ButtonType = car.CarState.ButtonEvent.Type
ButtonType = structs.CarState.ButtonEvent.Type
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])


Expand All @@ -35,18 +33,18 @@ def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
return val_steady


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] = []
def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, structs.CarState.ButtonEvent.Type],
unpressed_btn: int = 0) -> list[structs.CarState.ButtonEvent]:
events: list[structs.CarState.ButtonEvent] = []

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)))
events.append(structs.CarState.ButtonEvent(pressed=pressed,
type=buttons_dict.get(btn, ButtonType.unknown)))
return events


Expand Down Expand Up @@ -183,7 +181,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):


def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float,
torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float:
torque_params: structs.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float:
friction_interp = interp(
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
[-friction_threshold, friction_threshold],
Expand All @@ -203,8 +201,8 @@ def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False):
return CanData(addr, bytes(dat), bus)


def get_safety_config(safety_model, safety_param = None):
ret = car.CarParams.SafetyConfig.new_message()
def get_safety_config(safety_model: structs.CarParams.SafetyModel, safety_param: int = None) -> structs.CarParams.SafetyConfig:
ret = structs.CarParams.SafetyConfig()
ret.safetyModel = safety_model
if safety_param is not None:
ret.safetyParam = safety_param
Expand Down
3 changes: 2 additions & 1 deletion selfdrive/car/body/carcontroller.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
import copy
import numpy as np
from numbers import Number

Expand Down Expand Up @@ -132,7 +133,7 @@ def update(self, CC, CS, now_nanos):
can_sends = []
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))

new_actuators = CC.actuators.as_builder()
new_actuators = copy.copy(CC.actuators)
new_actuators.accel = torque_l
new_actuators.steer = torque_r
new_actuators.steerOutputCan = torque_r
Expand Down
8 changes: 4 additions & 4 deletions selfdrive/car/body/carstate.py
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car import structs
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.body.values import DBC


class CarState(CarStateBase):
def update(self, cp, *_):
ret = car.CarState.new_message()
def update(self, cp, *_) -> structs.CarState:
ret = structs.CarState()

ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
Expand All @@ -23,7 +23,7 @@ def update(self, cp, *_):
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100

# irrelevant for non-car
ret.gearShifter = car.CarState.GearShifter.drive
ret.gearShifter = structs.CarState.GearShifter.drive
ret.cruiseState.enabled = True
ret.cruiseState.available = True

Expand Down
4 changes: 2 additions & 2 deletions selfdrive/car/body/fingerprints.py
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
# ruff: noqa: E501
from cereal import car
from openpilot.selfdrive.car.structs import CarParams
from openpilot.selfdrive.car.body.values import CAR

Ecu = car.CarParams.Ecu
Ecu = CarParams.Ecu

# debug ecu fw version is the git hash of the firmware

Expand Down
9 changes: 4 additions & 5 deletions selfdrive/car/body/interface.py
Original file line number Diff line number Diff line change
@@ -1,16 +1,15 @@
import math
from cereal import car
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car import get_safety_config, structs
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM


class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experimental_long, docs) -> structs.CarParams:
ret.notCar = True
ret.carName = "body"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)]
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.body)]

ret.minSteerSpeed = -math.inf
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value
Expand All @@ -21,6 +20,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):

ret.radarUnavailable = True
ret.openpilotLongitudinalControl = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.steerControlType = structs.CarParams.SteerControlType.angle

return ret
4 changes: 2 additions & 2 deletions selfdrive/car/body/values.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
from cereal import car
from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict
from openpilot.selfdrive.car.structs import CarParams
from openpilot.selfdrive.car.docs_definitions import CarDocs
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries

Ecu = car.CarParams.Ecu
Ecu = CarParams.Ecu

SPEED_FROM_RPM = 0.008587

Expand Down
16 changes: 8 additions & 8 deletions selfdrive/car/car_helpers.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
import os
import time

from cereal import car
from openpilot.selfdrive.car import carlog, gen_empty_fingerprint
from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable
from openpilot.selfdrive.car.structs import CarParams
from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from openpilot.selfdrive.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car
from openpilot.selfdrive.car.interfaces import get_interface_attr
Expand Down Expand Up @@ -82,7 +82,7 @@ def can_fingerprint(can_recv: CanRecvCallable) -> tuple[str | None, dict[int, di

# **** for use live only ****
def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int,
cached_params: type[car.CarParams] | None) -> tuple[str | None, dict, str, list, int, bool]:
cached_params: CarParams | None) -> tuple[str | None, dict, str, list[CarParams.CarFw], CarParams.FingerprintSource, bool]:
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
Expand Down Expand Up @@ -129,17 +129,17 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu
car_fingerprint, finger = can_fingerprint(can_recv)

exact_match = True
source = car.CarParams.FingerprintSource.can
source = CarParams.FingerprintSource.can

# If FW query returns exactly 1 candidate, use it
if len(fw_candidates) == 1:
car_fingerprint = list(fw_candidates)[0]
source = car.CarParams.FingerprintSource.fw
source = CarParams.FingerprintSource.fw
exact_match = exact_fw_match

if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
source = CarParams.FingerprintSource.fixed

carlog.error({"event": "fingerprinted", "car_fingerprint": str(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,
Expand All @@ -148,21 +148,21 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu
return car_fingerprint, finger, vin, car_fw, source, exact_match


def get_car_interface(CP):
def get_car_interface(CP: CarParams):
CarInterface, CarController, CarState = interfaces[CP.carFingerprint]
return CarInterface(CP, CarController, CarState)


def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, experimental_long_allowed: bool,
num_pandas: int = 1, cached_params: type[car.CarParams] | None = None):
num_pandas: int = 1, cached_params: CarParams | None = None):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params)

if candidate is None:
carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)})
candidate = "MOCK"

CarInterface, _, _ = interfaces[candidate]
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False)
CP: CarParams = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
Expand Down
32 changes: 16 additions & 16 deletions selfdrive/car/car_specific.py
Original file line number Diff line number Diff line change
@@ -1,16 +1,16 @@
from cereal import car
import cereal.messaging as messaging
from openpilot.selfdrive.car import DT_CTRL
from openpilot.selfdrive.car.interfaces import MAX_CTRL_SPEED
from openpilot.selfdrive.car import DT_CTRL, structs
from openpilot.selfdrive.car.interfaces import MAX_CTRL_SPEED, CarStateBase, CarControllerBase
from openpilot.selfdrive.car.volkswagen.values import CarControllerParams as VWCarControllerParams
from openpilot.selfdrive.car.hyundai.interface import ENABLE_BUTTONS as HYUNDAI_ENABLE_BUTTONS

from openpilot.selfdrive.controls.lib.events import Events

ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter
EventName = car.CarEvent.EventName
NetworkLocation = car.CarParams.NetworkLocation
NetworkLocation = structs.CarParams.NetworkLocation


# TODO: the goal is to abstract this file into the CarState struct and make events generic
Expand All @@ -29,15 +29,15 @@ def update(self, CS: car.CarState):


class CarSpecificEvents:
def __init__(self, CP: car.CarParams):
def __init__(self, CP: structs.CarParams):
self.CP = CP

self.steering_unpressed = 0
self.low_speed_alert = False
self.no_steer_warning = False
self.silent_steer_warning = True

def update(self, CS, CS_prev, CC, CC_prev):
def update(self, CS: CarStateBase, CS_prev: car.CarState, CC: CarControllerBase, CC_prev: car.CarControl):
if self.CP.carName in ('body', 'mock'):
events = Events()

Expand All @@ -50,15 +50,15 @@ def update(self, CS, CS_prev, CC, CC_prev):
elif self.CP.carName == 'nissan':
events = self.create_common_events(CS.out, CS_prev, extra_gears=[GearShifter.brake])

if CS.lkas_enabled:
if CS.lkas_enabled: # type: ignore[attr-defined]
events.add(EventName.invalidLkasSetting)

elif self.CP.carName == 'mazda':
events = self.create_common_events(CS.out, CS_prev)

if CS.lkas_disabled:
if CS.lkas_disabled: # type: ignore[attr-defined]
events.add(EventName.lkasDisabled)
elif CS.low_speed_alert:
elif CS.low_speed_alert: # type: ignore[attr-defined]
events.add(EventName.belowSteerSpeed)

elif self.CP.carName == 'chrysler':
Expand Down Expand Up @@ -99,7 +99,7 @@ def update(self, CS, CS_prev, CC, CC_prev):
if self.CP.openpilotLongitudinalControl:
if CS.out.cruiseState.standstill and not CS.out.brakePressed:
events.add(EventName.resumeRequired)
if CS.low_speed_lockout:
if CS.low_speed_lockout: # type: ignore[attr-defined]
events.add(EventName.lowSpeedLockout)
if CS.out.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
Expand All @@ -121,7 +121,7 @@ def update(self, CS, CS_prev, CC, CC_prev):

# Enabling at a standstill with brake is allowed
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
below_min_enable_speed = CS.out.vEgo < self.CP.minEnableSpeed or CS.moving_backward
below_min_enable_speed = CS.out.vEgo < self.CP.minEnableSpeed or CS.moving_backward # type: ignore[attr-defined]
if below_min_enable_speed and not (CS.out.standstill and CS.out.brake >= 20 and
self.CP.networkLocation == NetworkLocation.fwdCamera):
events.add(EventName.belowEngageSpeed)
Expand Down Expand Up @@ -149,14 +149,14 @@ def update(self, CS, CS_prev, CC, CC_prev):
if CC_prev.enabled and CS.out.vEgo < self.CP.minEnableSpeed:
events.add(EventName.speedTooLow)

if CC.eps_timer_soft_disable_alert:
if CC.eps_timer_soft_disable_alert: # type: ignore[attr-defined]
events.add(EventName.steerTimeLimit)

elif self.CP.carName == 'hyundai':
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
# Main button also can trigger an engagement on these cars
allow_enable = any(btn in HYUNDAI_ENABLE_BUTTONS for btn in CS.cruise_buttons) or any(CS.main_buttons)
allow_enable = any(btn in HYUNDAI_ENABLE_BUTTONS for btn in CS.cruise_buttons) or any(CS.main_buttons) # type: ignore[attr-defined]
events = self.create_common_events(CS.out, CS_prev, pcm_enable=self.CP.pcmCruise, allow_enable=allow_enable)

# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
Expand All @@ -172,8 +172,8 @@ def update(self, CS, CS_prev, CC, CC_prev):

return events

def create_common_events(self, CS, CS_prev, extra_gears=None, pcm_enable=True, allow_enable=True,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
events = Events()

if CS.doorOpen:
Expand Down
Loading
Loading