mawei/opendbc-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/opendbc.git synced 2026-02-18 13:03:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Modular Assistive Driving System (MADS) (#29)

Browse Source 
* fix

* clearer

* cleanup

* more fix

* hkg dbc

* typo!

* Add Custom MIT License (#26)

* missed

* better

* more fixes

* try this out

* inherit in carcontroller properly

* hyundai: main button handling

* always disable

* add main check for pcm cruise

* revert

* sunnyParams

* Move car-specific changes to opendbc

* no need

* more fixes

* more!

* final?

* static analysis

* use new cereal

* rename to lkas_button

* rename

* mads base for cars

* add lkas for ford

* enabled <-> active

* MUST REMOVE test process replay

* Revert "MUST REMOVE test process replay"

This reverts commit 6dde2c8435b0e09158ab455aa215a573f5212c11.

* subaru

* ruff

* more subaru

* toyota

* add them

* mypy

* fix

* update name

* FCA

* assign directly

* init directly

* missing

* not yet

* missed

* no longer needed

* missed hd

* fix

* move to generator

* more nissan

* Apply suggestions from code review

* no need

* Revert "no need"

This reverts commit 6156c62113d9abb626014947a9066b5580f6460a.

* hyundai: move main logic out of main carstate

* move around

* move lkas and lfa icon logic to mads base

* Parse more flags from alt exp, more tests, hyundai main cruise allowed

* license

* add code spell ignore

* fix icon

* remove toyota lta status for lkas, causes weird behaviors

* parse signals inside mads methods

* more codes in mads childs

* Update opendbc/sunnypilot/car/hyundai/escc.py

* revert

* type hint

* test type hint

* more type hint

* no

* needs to be in carstate

* in another PR

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
DevTekVE
2024-12-16 09:33:18 +01:00
committed by GitHub
parent 378967043d
commit 5d399457c8
27 changed files with 630 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.codespellignore Normal file
View File

@@ -0,0 +1 @@
Wen

2
.pre-commit-config.yaml
View File

@@ -13,6 +13,8 @@ repos:
hooks:
- id: codespell
exclude: '(\.dbc|LICENSE\.md)$'
args:
- --ignore-words=.codespellignore
- repo: https://github.com/pre-commit/mirrors-mypy
rev: v1.11.1
hooks:

2
opendbc/car/car.capnp
View File

@@ -351,6 +351,7 @@ struct CarControl {
cruiseControl @4 :CruiseControl;
hudControl @5 :HUDControl;
madsEnabled @7 :Bool;
sunnypilotParams @8 :UInt32;
struct Actuators {
@@ -430,7 +431,6 @@ struct CarControl {
gasDEPRECATED @1 :Float32;
brakeDEPRECATED @2 :Float32;
steeringTorqueDEPRECATED @3 :Float32;
activeDEPRECATED @7 :Bool;
pitchDEPRECATED @9 :Float32;
actuatorsOutputDEPRECATED @10 :Actuators;
}

13
opendbc/car/chrysler/carcontroller.py
View File

@@ -4,10 +4,13 @@ from opendbc.car.chrysler import chryslercan
from opendbc.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
from opendbc.car.interfaces import CarControllerBase
from opendbc.sunnypilot.car.chrysler.mads import MadsCarController
class CarController(CarControllerBase):
class CarController(CarControllerBase, MadsCarController):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
CarControllerBase.__init__(self, dbc_names, CP)
MadsCarController.__init__(self)
self.apply_steer_last = 0
self.hud_count = 0
@@ -19,6 +22,7 @@ class CarController(CarControllerBase):
self.params = CarControllerParams(CP)
def update(self, CC, CS, now_nanos):
MadsCarController.update(self, CC, CS)
can_sends = []
lkas_active = CC.latActive and self.lkas_control_bit_prev
@@ -41,7 +45,7 @@ class CarController(CarControllerBase):
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
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, CS.lkas_car_model, CS.auto_high_beam, self.mads))
self.hud_count += 1
# steering
@@ -74,6 +78,9 @@ class CarController(CarControllerBase):
can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
can_sends.append(MadsCarController.create_lkas_heartbit(self.packer, CS.lkas_heartbit, self.mads))
self.frame += 1
new_actuators = CC.actuators.as_builder()

16
opendbc/car/chrysler/carstate.py
View File

@@ -5,12 +5,15 @@ from opendbc.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import CarStateBase
from opendbc.sunnypilot.car.chrysler.mads import MadsCarState
ButtonType = structs.CarState.ButtonEvent.Type
class CarState(CarStateBase):
class CarState(CarStateBase, MadsCarState):
def __init__(self, CP):
super().__init__(CP)
CarStateBase.__init__(self, CP)
MadsCarState.__init__(self, CP)
self.CP = CP
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
@@ -104,7 +107,12 @@ class CarState(CarStateBase):
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
MadsCarState.update_mads(self, ret, can_parsers)
ret.buttonEvents = [
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}),
]
return ret
@@ -154,6 +162,8 @@ class CarState(CarStateBase):
if CP.carFingerprint in RAM_CARS:
cam_messages += CarState.get_cruise_messages()
MadsCarState.get_parser(CP, pt_messages, cam_messages)
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),

7
opendbc/car/chrysler/chryslercan.py
View File

@@ -4,7 +4,7 @@ from opendbc.car.chrysler.values import RAM_CARS
GearShifter = structs.CarState.GearShifter
VisualAlert = structs.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam):
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, mads):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
@@ -27,7 +27,10 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
# 7 Normal
# 6 lane departure place hands on wheel
color = 2 if lkas_active else 1
if mads.enable_mads:
color = 2 if lkas_active else 1 if mads.paused else 0
else:
color = 2 if lkas_active else 1
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0

13
opendbc/car/ford/carstate.py
View File

@@ -6,14 +6,17 @@ from opendbc.car.ford.fordcan import CanBus
from opendbc.car.ford.values import DBC, CarControllerParams, FordFlags
from opendbc.car.interfaces import CarStateBase
from opendbc.sunnypilot.car.ford.mads import MadsCarState
ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter
TransmissionType = structs.CarParams.TransmissionType
class CarState(CarStateBase):
class CarState(CarStateBase, MadsCarState):
def __init__(self, CP):
super().__init__(CP)
CarStateBase.__init__(self, CP)
MadsCarState.__init__(self, CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
@@ -110,7 +113,11 @@ class CarState(CarStateBase):
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
MadsCarState.update_mads(self, ret, can_parsers)
ret.buttonEvents = [
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})]
return ret

19
opendbc/car/hyundai/carcontroller.py
View File

@@ -9,6 +9,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParam
from opendbc.car.interfaces import CarControllerBase
from opendbc.sunnypilot.car.hyundai.escc import EsccCarController
from opendbc.sunnypilot.car.hyundai.mads import MadsCarController
VisualAlert = structs.CarControl.HUDControl.VisualAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
@@ -44,10 +45,11 @@ def process_hud_alert(enabled, fingerprint, hud_control):
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController(CarControllerBase, EsccCarController):
class CarController(CarControllerBase, EsccCarController, MadsCarController):
def __init__(self, dbc_names, CP):
CarControllerBase.__init__(self, dbc_names, CP)
EsccCarController.__init__(self, CP)
MadsCarController.__init__(self)
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_names[Bus.pt])
@@ -60,6 +62,7 @@ class CarController(CarControllerBase, EsccCarController):
def update(self, CC, CS, now_nanos):
EsccCarController.update(self, CS)
MadsCarController.update(self, self.CP, CC, self.frame)
actuators = CC.actuators
hud_control = CC.hudControl
@@ -112,7 +115,7 @@ class CarController(CarControllerBase, EsccCarController):
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer, self.lkas_icon))
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2:
@@ -121,7 +124,7 @@ class CarController(CarControllerBase, EsccCarController):
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, self.lfa_icon))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -132,7 +135,8 @@ class CarController(CarControllerBase, EsccCarController):
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control))
set_speed_in_units, hud_control,
CS.main_cruise_enabled))
self.accel_last = accel
else:
# button presses
@@ -141,7 +145,8 @@ class CarController(CarControllerBase, EsccCarController):
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
left_lane_warning, right_lane_warning,
self.lkas_icon))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
@@ -153,11 +158,11 @@ class CarController(CarControllerBase, EsccCarController):
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca,
self.ESCC))
CS.main_cruise_enabled, self.ESCC))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.lfa_icon))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:

21
opendbc/car/hyundai/carstate.py
View File

@@ -11,6 +11,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, DBC, Buttons, CarContr
from opendbc.car.interfaces import CarStateBase
from opendbc.sunnypilot.car.hyundai.escc import EsccCarStateBase
from opendbc.sunnypilot.car.hyundai.mads import MadsCarState
ButtonType = structs.CarState.ButtonEvent.Type
@@ -22,10 +23,11 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
class CarState(CarStateBase, EsccCarStateBase):
class CarState(CarStateBase, EsccCarStateBase, MadsCarState):
def __init__(self, CP):
CarStateBase.__init__(self, CP)
EsccCarStateBase.__init__(self)
MadsCarState.__init__(self, CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
@@ -178,8 +180,14 @@ class CarState(CarStateBase, EsccCarStateBase):
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
MadsCarState.update_mads(self, ret, can_parsers)
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}),
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})]
if self.CP.openpilotLongitudinalControl:
ret.cruiseState.available = self.get_main_cruise(ret)
return ret
@@ -267,8 +275,14 @@ class CarState(CarStateBase, EsccCarStateBase):
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
MadsCarState.update_mads_canfd(self, ret, can_parsers)
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}),
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})]
if self.CP.openpilotLongitudinalControl:
ret.cruiseState.available = self.get_main_cruise(ret)
return ret
@@ -384,6 +398,7 @@ class CarState(CarStateBase, EsccCarStateBase):
if CP.flags & HyundaiFlags.USE_FCA.value:
cam_messages.append(("FCA11", 50))
MadsCarState.get_parser(CP, pt_messages)
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),

15
opendbc/car/hyundai/hyundaican.py
View File

@@ -8,7 +8,8 @@ hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def create_lkas11(packer, frame, CP, apply_steer, steer_req,
torque_fault, lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane,
left_lane_depart, right_lane_depart):
left_lane_depart, right_lane_depart,
lkas_icon):
values = {s: lkas11[s] for s in [
"CF_Lkas_LdwsActivemode",
"CF_Lkas_LdwsSysState",
@@ -50,7 +51,7 @@ def create_lkas11(packer, frame, CP, apply_steer, steer_req,
# FcwOpt_USM 2 = Green car + lanes
# FcwOpt_USM 1 = White car + lanes
# FcwOpt_USM 0 = No car + lanes
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
values["CF_Lkas_FcwOpt_USM"] = lkas_icon
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
@@ -67,7 +68,7 @@ def create_lkas11(packer, frame, CP, apply_steer, steer_req,
# SysState 1-2 = white car + lanes
# SysState 3 = green car + lanes, green steering wheel
# SysState 4 = green car + lanes
values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1
values["CF_Lkas_LdwsSysState"] = lkas_icon
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
# these have no effect
@@ -119,18 +120,18 @@ def create_clu11(packer, frame, clu11, button, CP):
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled):
def create_lfahda_mfc(packer, enabled, lfa_icon):
values = {
"LFA_Icon_State": 2 if enabled else 0,
"LFA_Icon_State": lfa_icon,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
ESCC: EnhancedSmartCruiseControl = None):
main_cruise_enabled, ESCC: EnhancedSmartCruiseControl = None):
def get_scc11_values():
return {
"MainMode_ACC": 1,
"MainMode_ACC": 1 if main_cruise_enabled else 0,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,

12
opendbc/car/hyundai/hyundaicanfd.py
View File

@@ -34,13 +34,13 @@ class CanBus(CanBusBase):
return self._cam
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, lkas_icon):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"LKA_ICON": lkas_icon,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
@@ -112,15 +112,15 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
def create_lfahda_cluster(packer, CAN, enabled, lfa_icon):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
"LFA_ICON": lfa_icon,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, main_cruise_enabled):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
@@ -131,7 +131,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"MainMode_ACC": 1 if main_cruise_enabled else 0,
"StopReq": 1 if stopping else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,

3
opendbc/car/hyundai/interface.py
View File

@@ -95,6 +95,9 @@ class CarInterface(CarInterfaceBase):
if ret.flags & HyundaiFlags.CAMERA_SCC:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
if 0x391 in fingerprint[0]:
ret.sunnypilotFlags |= HyundaiFlagsSP.HAS_LFA_BUTTON.value
# Common lateral control setup
ret.centerToFront = ret.wheelbase * 0.4

4
opendbc/car/nissan/interface.py
View File

@@ -23,4 +23,8 @@ class CarInterface(CarInterfaceBase):
# Altima has EPS on C-CAN unlike the others that have it on V-CAN
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS
# Used for panda safety and tests
if candidate in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC):
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_NISSAN_LEAF
return ret

9
opendbc/car/subaru/carstate.py
View File

@@ -7,10 +7,13 @@ from opendbc.car.interfaces import CarStateBase
from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags
from opendbc.car import CanSignalRateCalculator
from opendbc.sunnypilot.car.subaru.mads import MadsCarState
class CarState(CarStateBase):
class CarState(CarStateBase, MadsCarState):
def __init__(self, CP):
super().__init__(CP)
CarStateBase.__init__(self, CP)
MadsCarState.__init__(self, CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.shifter_values = can_define.dv["Transmission"]["Gear"]
@@ -128,6 +131,8 @@ class CarState(CarStateBase):
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
MadsCarState.update_mads(self, ret, can_parsers)
return ret
@staticmethod

16
opendbc/car/toyota/carstate.py
View File

@@ -10,6 +10,8 @@ from opendbc.car.interfaces import CarStateBase
from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
from opendbc.sunnypilot.car.toyota.mads import MadsCarState
ButtonType = structs.CarState.ButtonEvent.Type
SteerControlType = structs.CarParams.SteerControlType
@@ -25,9 +27,10 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
PERM_STEER_FAULTS = (3, 17)
class CarState(CarStateBase):
class CarState(CarStateBase, MadsCarState):
def __init__(self, CP):
super().__init__(CP)
CarStateBase.__init__(self, CP)
MadsCarState.__init__(self, CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
@@ -188,7 +191,14 @@ class CarState(CarStateBase):
prev_distance_button = self.distance_button
self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
self.distance_button_events = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
MadsCarState.update_mads(self, ret, can_parsers)
ret.buttonEvents = [
*self.distance_button_events,
*self.lkas_button_events,
]
return ret

5
opendbc/sunnypilot/__init__.py
View File

@@ -0,0 +1,5 @@
from enum import IntFlag
class SunnypilotParamFlags(IntFlag):
ENABLE_MADS = 1

0
opendbc/sunnypilot/car/chrysler/__init__.py Normal file
View File

114
opendbc/sunnypilot/car/chrysler/mads.py Normal file
View File

@@ -0,0 +1,114 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import StrEnum
from collections import namedtuple
from opendbc.car import Bus, structs
from opendbc.car.chrysler.values import RAM_CARS
from opendbc.sunnypilot import SunnypilotParamFlags
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
MadsDataSP = namedtuple("MadsDataSP",
["enable_mads", "paused", "lkas_disabled"])
ButtonType = structs.CarState.ButtonEvent.Type
class MadsCarController:
def __init__(self):
super().__init__()
self.mads = MadsDataSP(False, False, False)
@staticmethod
def create_lkas_heartbit(packer, lkas_heartbit, mads):
# LKAS_HEARTBIT (0x2D9) LKAS heartbeat
values = {s: lkas_heartbit[s] for s in [
"LKAS_DISABLED",
"AUTO_HIGH_BEAM",
"FORWARD_1",
"FORWARD_2",
"FORWARD_3",
]}
if mads.enable_mads:
values["LKAS_DISABLED"] = 1 if mads.lkas_disabled else 0
return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
@staticmethod
def mads_status_update(CC: structs.CarControl, CS) -> MadsDataSP:
enable_mads = CC.sunnypilotParams & SunnypilotParamFlags.ENABLE_MADS
paused = CC.madsEnabled and not CC.latActive
if any(be.type == ButtonType.lkas and be.pressed for be in CS.out.buttonEvents):
CS.lkas_disabled = not CS.lkas_disabled
return MadsDataSP(enable_mads, paused, CS.lkas_disabled)
def update(self, CC: structs.CarControl, CS) -> None:
self.mads = self.mads_status_update(CC, CS)
class MadsCarState(MadsCarStateBase):
def __init__(self, CP: structs.CarParams):
super().__init__(CP)
self.lkas_heartbit = 0
self.init_lkas_disabled = False
self.lkas_disabled = False
@staticmethod
def get_parser(CP, pt_messages, cam_messages) -> None:
if CP.carFingerprint in RAM_CARS:
pt_messages += [
("Center_Stack_1", 1),
("Center_Stack_2", 1),
]
else:
pt_messages.append(("TRACTION_BUTTON", 1))
cam_messages.append(("LKAS_HEARTBIT", 1))
def get_lkas_button(self, cp, cp_cam):
if self.CP.carFingerprint in RAM_CARS:
lkas_button = cp.vl["Center_Stack_1"]["LKAS_Button"] or cp.vl["Center_Stack_2"]["LKAS_Button"]
else:
lkas_button = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"]
self.lkas_heartbit = cp_cam.vl["LKAS_HEARTBIT"]
if not self.init_lkas_disabled:
self.lkas_disabled = cp_cam.vl["LKAS_HEARTBIT"]["LKAS_DISABLED"]
self.init_lkas_disabled = True
return lkas_button
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
self.prev_lkas_button = self.lkas_button
self.lkas_button = self.get_lkas_button(cp, cp_cam)

0
opendbc/sunnypilot/car/ford/__init__.py Normal file
View File

43
opendbc/sunnypilot/car/ford/mads.py Normal file
View File

@@ -0,0 +1,43 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import StrEnum
from opendbc.car import Bus,structs
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
class MadsCarState(MadsCarStateBase):
def __init__(self, CP: structs.CarParams):
super().__init__(CP)
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp = can_parsers[Bus.pt]
self.prev_lkas_button = self.lkas_button
self.lkas_button = cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]

137
opendbc/sunnypilot/car/hyundai/mads.py Normal file
View File

@@ -0,0 +1,137 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import StrEnum
from collections import namedtuple
from opendbc.car import Bus, DT_CTRL, structs
from opendbc.car.hyundai.values import CAR
from opendbc.sunnypilot import SunnypilotParamFlags
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
ButtonType = structs.CarState.ButtonEvent.Type
MadsDataSP = namedtuple("MadsDataSP",
["enable_mads", "lat_active", "disengaging", "paused"])
class MadsCarController:
def __init__(self):
super().__init__()
self.mads = MadsDataSP(False, False, False, False)
self.lat_disengage_blink = 0
self.lat_disengage_init = False
self.prev_lat_active = False
self.lkas_icon = 0
self.lfa_icon = 0
# display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive
def mads_status_update(self, CC: structs.CarControl, frame: int) -> MadsDataSP:
enable_mads = CC.sunnypilotParams & SunnypilotParamFlags.ENABLE_MADS
if CC.latActive:
self.lat_disengage_init = False
elif self.prev_lat_active:
self.lat_disengage_init = True
if not self.lat_disengage_init:
self.lat_disengage_blink = frame
paused = CC.madsEnabled and not CC.latActive
disengaging = (frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
self.prev_lat_active = CC.latActive
return MadsDataSP(enable_mads, CC.latActive, disengaging, paused)
def create_lkas_icon(self, CP: structs.CarParams, enabled: bool) -> int:
if self.mads.enable_mads:
lkas_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1
else:
lkas_icon = 2 if enabled else 1
# Override common signals for KIA_OPTIMA_G4 and KIA_OPTIMA_G4_FL
if CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
lkas_icon = 3 if (self.mads.lat_active if self.mads.enable_mads else enabled) else 1
return lkas_icon
def create_lfa_icon(self, enabled: bool) -> int:
if self.mads.enable_mads:
lfa_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1 if self.mads.paused else 0
else:
lfa_icon = 2 if enabled else 0
return lfa_icon
def update(self, CP: structs.CarParams, CC: structs.CarControl, frame: int) -> None:
self.mads = self.mads_status_update(CC, frame)
self.lkas_icon = self.create_lkas_icon(CP, CC.enabled)
self.lfa_icon = self.create_lfa_icon(CC.enabled)
class MadsCarState(MadsCarStateBase):
def __init__(self, CP: structs.CarParams):
super().__init__(CP)
self.main_cruise_enabled: bool = False
self.cruise_btns_msg_canfd = None
@staticmethod
def get_parser(CP, pt_messages) -> None:
if CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON:
pt_messages.append(("BCM_PO_11", 50))
def get_main_cruise(self, ret: structs.CarState) -> bool:
if any(be.type == ButtonType.mainCruise and be.pressed for be in ret.buttonEvents) and \
(self.CP.sunnypilotFlags & HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE):
self.main_cruise_enabled = not self.main_cruise_enabled
return self.main_cruise_enabled if ret.cruiseState.available else False
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp = can_parsers[Bus.pt]
self.prev_lkas_button = self.lkas_button
if self.CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON:
self.lkas_button = cp.vl["BCM_PO_11"]["LFA_Pressed"]
def update_mads_canfd(self, ret, can_parsers) -> None:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
if not self.CP.openpilotLongitudinalControl:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
self.prev_lkas_button = self.lkas_button
lfa_button = "LFA_BTN" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "LKAS_BTN"
self.lkas_button = cp.vl[self.cruise_btns_msg_canfd][lfa_button]

28
opendbc/sunnypilot/car/hyundai/values.py
View File

@@ -1,3 +1,29 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import IntFlag
@@ -6,3 +32,5 @@ class HyundaiFlagsSP(IntFlag):
Flags for Hyundai specific quirks within sunnypilot.
"""
ENHANCED_SCC = 1
HAS_LFA_BUTTON = 2
LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2 ** 2

0
opendbc/sunnypilot/car/subaru/__init__.py Normal file
View File

67
opendbc/sunnypilot/car/subaru/mads.py Normal file
View File

@@ -0,0 +1,67 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import StrEnum
from opendbc.car import Bus, structs
from opendbc.car.subaru.values import SubaruFlags
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
ButtonType = structs.CarState.ButtonEvent.Type
class MadsCarState(MadsCarStateBase):
def __init__(self, CP: structs.CarParams):
super().__init__(CP)
@staticmethod
def create_lkas_button_events(cur_btn: int, prev_btn: int,
buttons_dict: dict[int, structs.CarState.ButtonEvent.Type]) -> list[structs.CarState.ButtonEvent]:
events: list[structs.CarState.ButtonEvent] = []
if cur_btn == prev_btn:
return events
state_changes = [
{"pressed": prev_btn != cur_btn and cur_btn != 2 and not (prev_btn == 2 and cur_btn == 1)},
{"pressed": prev_btn != cur_btn and cur_btn == 2 and cur_btn != 1},
]
for change in state_changes:
if change["pressed"]:
events.append(structs.CarState.ButtonEvent(pressed=change["pressed"],
type=buttons_dict.get(cur_btn, ButtonType.unknown)))
return events
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp_cam = can_parsers[Bus.cam]
self.prev_lkas_button = self.lkas_button
if not self.CP.flags & SubaruFlags.PREGLOBAL:
self.lkas_button = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
ret.buttonEvents = self.create_lkas_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})

0
opendbc/sunnypilot/car/toyota/__init__.py Normal file
View File

80
opendbc/sunnypilot/car/toyota/mads.py Normal file
View File

@@ -0,0 +1,80 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from enum import StrEnum
from opendbc.car import Bus, structs
from opendbc.car.toyota.values import CAR
from opendbc.sunnypilot.mads_base import MadsCarStateBase
from opendbc.can.parser import CANParser
ButtonType = structs.CarState.ButtonEvent.Type
class MadsCarState(MadsCarStateBase):
distance_button_events: list[structs.CarState.ButtonEvent]
lkas_button_events: list[structs.CarState.ButtonEvent]
def __init__(self, CP: structs.CarParams):
super().__init__(CP)
self.distance_button_events = []
self.lkas_button_events = []
@staticmethod
def get_lkas_button(cp_cam):
lkas_button = cp_cam.vl["LKAS_HUD"]["LKAS_STATUS"]
return lkas_button
@staticmethod
def create_lkas_button_events(cur_btn: int, prev_btn: int,
buttons_dict: dict[int, structs.CarState.ButtonEvent.Type]) -> list[structs.CarState.ButtonEvent]:
events: list[structs.CarState.ButtonEvent] = []
if cur_btn == prev_btn:
return events
state_changes = [
{"pressed": bool(not prev_btn and cur_btn)},
{"pressed": bool(prev_btn == 1 and not cur_btn)},
]
for change in state_changes:
if change["pressed"]:
events.append(structs.CarState.ButtonEvent(pressed=change["pressed"],
type=buttons_dict.get(cur_btn, ButtonType.unknown)))
return events
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
cp_cam = can_parsers[Bus.cam]
self.prev_lkas_button = self.lkas_button
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
self.lkas_button = self.get_lkas_button(cp_cam)
self.lkas_button_events = self.create_lkas_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})

43
opendbc/sunnypilot/mads_base.py Normal file
View File

@@ -0,0 +1,43 @@
"""
The MIT License
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
Last updated: July 29, 2024
"""
from abc import abstractmethod, ABC
from enum import StrEnum
from opendbc.car import structs
from opendbc.can.parser import CANParser
class MadsCarStateBase(ABC):
def __init__(self, CP: structs.CarParams):
self.CP = CP
self.lkas_button = 0
self.prev_lkas_button = 0
@abstractmethod
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
pass