mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-30 04:03:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

MADS: Support all cars (#13)

Browse Source 
* sunnypilot: MADS - Base + HKG

* fix upstream conflicts

* Don't miss this

* Missed it

* Don't initiate lane change when braking

* HKG: better cluster state

* HKG: only blink when disengaging lateral

* HKG: Optima's icons

* HKG: update vars and blink for CAN-FD

* HKG: don't get stuck

* obsolete

* remove this for now

* border color should sync

* change border color

* simplify things

* HKG: update events

* Honda: MADS

* HKG: cleanup

* Toyota: MADS

* Revert "remove this for now"

This reverts commit 76ddd435084e051ced9a343372900ced96180989.

* Revert "HKG: cleanup"

This reverts commit e1ab96c6f7634508b5f3820868d22f4694361622.

* Toyota: use the working one

* same colors

* block resume press if no previous set speed

* HKG: handle events better

* Honda: handle events better

* Toyota: handle events better

* Volkswagen MQB & PQ: MADS

* Subaru: MADS

* FCA: MADS

* add another argument for per port specifics

* GM: MADS

* Toyota: missed this

* Mazda: MADS

* Nissan: MADS

* have to use this

* pass events to the next

* move it around

* move some stuff around

* cleanup

* simplify and check if cruise is initialized with non PCM

* don't think we need this anymore

* Volkswagen: These 2 buttons don't get used

* check resume available differently

* check better

* simplify

* Mazda: missed one update check

* HKG: Move carstate logic to interface

* check from ret instead

* cruise state enabled spam bug

* check pcm

* set it earlier

* own cruise initialize check

* fixup! own cruise initialize check

* more inclusive

* HKG: redundant

* Honda: Move carstate logic to interface

* only check cruise speed when not PCM

* Toyota: Move carstate logic to interface

* fixup! HKG: Move carstate logic to interface

* fixup! Honda: Move carstate logic to interface

* fixup! Honda: Move carstate logic to interface

* Volkswagen: Move carstate logic to interface

* check if cruise has ever been enabled in state machine

* already exists

* just one more

* do not allow steering in certain situations

* Subaru: Move carstate logic to interface

* FCA: Move carstate logic to interface

* GM: Move carstate logic to interface

* Mazda: Move carstate logic to interface

* Nissan: Move carstate logic to interface

* HKG: move init

* declare at once

* cleanup variables

* prevent stuck engaged

* initialize for all cars

* slimmer

* no entry for brake press engage

* Revert "no entry for brake press engage"

This reverts commit 9e6e1ca5000c213017091765a0e50d2d794cebe9.

* Toyota: Change the logic

* Nissan: Cleanup carstate

* HKG: only disengage when cruise intially not available

* Revert "HKG: only disengage when cruise intially not available"

This reverts commit 123cce1818ea7285cbd433524c4f0e80e7c99cb4.

* allow press anytime

* cancel cruise when control wants to cancel

* remove some stuff

* wrong one

* can't do that

* would this fix it?
This commit is contained in:
Jason Wen
2023-01-29 15:27:49 -05:00
committed by GitHub
parent f874bf9cff
commit e84a0326da
46 changed files with 947 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+5
View File
@@ -199,6 +199,11 @@ std::unordered_map<std::string, uint32_t> keys = {
{"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
{"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
{"AccMadsCombo", PERSISTENT},
{"BelowSpeedPause", PERSISTENT},
{"DisengageLateralOnBrake", PERSISTENT},
{"EnableMads", PERSISTENT},
};
} // namespace
selfdrive/boardd/boardd.cc
+1
View File
@@ -366,6 +366,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
ps.setIgnitionLine(health.ignition_line_pkt);
ps.setIgnitionCan(health.ignition_can_pkt);
ps.setControlsAllowed(health.controls_allowed_pkt);
ps.setControlsAllowedLong(health.controls_allowed_long_pkt);
ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt);
ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
selfdrive/car/chrysler/carcontroller.py
+1 -1
View File
@@ -41,7 +41,7 @@ class CarController:
# 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(create_lkas_hud(self.packer, self.CP, lkas_active, CS.madsEnabled, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
self.hud_count += 1
# steering
selfdrive/car/chrysler/carstate.py
+4 -2
View File
@@ -25,6 +25,8 @@ class CarState(CarStateBase):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
# lock info
ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"],
cp.vl["BCM_1"]["DOOR_OPEN_FR"],
@@ -58,8 +60,8 @@ class CarState(CarStateBase):
)
# button presses
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1
# steering wheel
selfdrive/car/chrysler/chryslercan.py
+2 -2
View File
@@ -4,7 +4,7 @@ from selfdrive.car.chrysler.values import RAM_CARS
GearShifter = car.CarState.GearShifter
VisualAlert = car.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, mads_enabled, hud_alert, hud_count, car_model, auto_high_beam):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
@@ -27,7 +27,7 @@ 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
color = 2 if lkas_active else 1 if mads_enabled and not lkas_active else 0
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0
selfdrive/car/chrysler/interface.py
+49 -1
View File
@@ -5,6 +5,10 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@staticmethod
@@ -85,8 +89,52 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
buttonEvents = []
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if self.CS.out.cruiseState.enabled: # CANCEL
if not ret.cruiseState.enabled:
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0:
if ret.gasPressed and not ret.cruiseState.enabled:
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# CANCEL
if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.cancel
buttonEvents.append(be)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5):
selfdrive/car/gm/carcontroller.py
+2 -2
View File
@@ -99,12 +99,12 @@ class CarController:
friction_brake_bus = CanBus.POWERTRAIN
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled and CS.out.cruiseState.enabled, at_full_stop))
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP))
# Send dashboard UI commands (ACC status)
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled and CS.out.cruiseState.enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
selfdrive/car/gm/carstate.py
+10 -2
View File
@@ -21,6 +21,9 @@ class CarState(CarStateBase):
self.camera_lka_steering_cmd_counter = 0
self.buttons_counter = 0
self.lkas_enabled = False
self.prev_lkas_enabled = False
def update(self, pt_cp, cam_cp, loopback_cp):
ret = car.CarState.new_message()
@@ -29,6 +32,8 @@ class CarState(CarStateBase):
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
self.prev_mads_enabled = self.mads_enabled
self.prev_lkas_enabled = self.lkas_enabled
# Variables used for avoiding LKAS faults
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
@@ -46,6 +51,8 @@ class CarState(CarStateBase):
# sample rear wheel speeds, standstill=True if ECM allows engagement with brake
ret.standstill = ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
ret.gearShifter = self.parse_gear_shifter("T")
else:
@@ -87,8 +94,8 @@ class CarState(CarStateBase):
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.leftBlinker = ret.leftBlinkerOn = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = ret.rightBlinkerOn = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
@@ -140,6 +147,7 @@ class CarState(CarStateBase):
("AcceleratorPedal2", "AcceleratorPedal2"),
("CruiseState", "AcceleratorPedal2"),
("ACCButtons", "ASCMSteeringButton"),
("LKAButton", "ASCMSteeringButton", 0),
("RollingCounter", "ASCMSteeringButton"),
("SteeringWheelAngle", "PSCMSteeringAngle"),
("SteeringWheelRate", "PSCMSteeringAngle"),
selfdrive/car/gm/interface.py
+51 -7
View File
@@ -205,21 +205,65 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)]
buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
# Handle ACCButtons changing buttons mid-press
if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS:
buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
ret.buttonEvents = buttonEvents
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in buttonEvents):
self.CS.accEnabled = True
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
buttonEvents, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.prev_lkas_enabled != 1 and self.CS.lkas_enabled == 1:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if any(b.type == ButtonType.cancel for b in buttonEvents):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0:
if ret.gasPressed and not ret.cruiseState.enabled:
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# The ECM allows enabling on falling edge of set, but only rising edge of resume
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
GearShifter.eco, GearShifter.manumatic],
pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
events.add(EventName.buttonEnable)
pcm_enable=False, enable_buttons=(ButtonType.decelCruise,))
#if not self.CP.pcmCruise:
# if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
# events.add(EventName.buttonEnable)
events, ret = self.create_sp_events(self.CS, ret, events, enable_pressed=self.CS.accEnabled,
enable_buttons=(ButtonType.decelCruise,))
# 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
@@ -229,7 +273,7 @@ class CarInterface(CarInterfaceBase):
events.add(EventName.belowEngageSpeed)
if ret.cruiseState.standstill:
events.add(EventName.resumeRequired)
if ret.vEgo < self.CP.minSteerSpeed:
if ret.vEgo < self.CP.minSteerSpeed and self.CS.madsEnabled:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
selfdrive/car/honda/carcontroller.py
+4 -4
View File
@@ -96,7 +96,7 @@ def process_hud_alert(hud_alert):
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "lead_visible",
"lanes_visible", "fcw", "acc_alert", "steer_required"])
"lanes_visible", "fcw", "acc_alert", "steer_required", "dashed_lanes"])
def rate_limit_steer(new_steer, last_steer):
@@ -217,7 +217,7 @@ class CarController:
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
stopping = actuators.longControlState == LongCtrlState.stopping
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, CC.longActive, self.accel, self.gas,
stopping, self.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
@@ -247,8 +247,8 @@ class CarController:
# Send dashboard UI commands.
if self.frame % 10 == 0:
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
hud_control.lanesVisible, fcw_display, acc_alert, steer_required, CS.madsEnabled and not CC.latActive)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive))
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed
selfdrive/car/honda/carstate.py
+2 -1
View File
@@ -165,6 +165,7 @@ class CarState(CarStateBase):
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting
self.prev_mads_enabled = self.mads_enabled
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
@@ -216,7 +217,7 @@ class CarState(CarStateBase):
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
selfdrive/car/honda/hondacan.py
+3 -2
View File
@@ -102,7 +102,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active):
commands = []
bus_pt = get_pt_bus(CP.carFingerprint)
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
@@ -135,7 +135,8 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
lkas_hud_values = {
'SET_ME_X41': 0x41,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes_visible,
'SOLID_LANES': lat_active,
'DASHED_LANES': hud.dashed_lanes,
'BEEP': 0,
}
selfdrive/car/honda/interface.py
+46 -14
View File
@@ -11,6 +11,7 @@ from selfdrive.car.disable_ecu import disable_ecu
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
@@ -317,28 +318,59 @@ class CarInterface(CarInterfaceBase):
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}))
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
buttonEvents, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.prev_cruise_setting != 1 and self.CS.cruise_setting == 1:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if any(b.type == ButtonType.cancel for b in buttonEvents):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0:
if ret.gasPressed and not ret.cruiseState.enabled:
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret, pcm_enable=False)
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed and not self.CS.madsEnabled:
events.add(EventName.belowEngageSpeed)
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.accel >= 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.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.accel >= 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)
selfdrive/car/hyundai/carcontroller.py
+28 -7
View File
@@ -54,6 +54,10 @@ class CarController:
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.disengage_blink = 0.
self.lat_disengage_init = False
self.lat_active_last = False
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
@@ -76,6 +80,20 @@ class CarController:
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
# show LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive
lateral_paused = CS.madsEnabled and not CC.latActive
if CC.latActive:
self.lat_disengage_init = False
elif self.lat_active_last:
self.lat_disengage_init = True
if not self.lat_disengage_init:
self.disengage_blink = self.frame
blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
self.lat_active_last = CC.latActive
can_sends = []
# *** common hyundai stuff ***
@@ -112,7 +130,8 @@ class CarController:
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer))
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer,
lateral_paused, blinking_icon))
# disable LFA on HDA2
if self.frame % 5 == 0 and hda2:
@@ -120,7 +139,8 @@ class CarController:
# 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.CP, CC.enabled))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, CC.latActive,
lateral_paused, blinking_icon))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -130,7 +150,7 @@ class CarController:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units))
self.accel_last = accel
else:
@@ -159,7 +179,8 @@ class CarController:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
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,
lateral_paused, blinking_icon))
if not self.CP.openpilotLongitudinalControl:
if CC.cruiseControl.cancel:
@@ -174,15 +195,15 @@ class CarController:
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override))
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, CS.mainEnabled))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022):
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive, lateral_paused, blinking_icon))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
selfdrive/car/hyundai/carstate.py
+27 -4
View File
@@ -42,6 +42,10 @@ class CarState(CarStateBase):
self.params = CarControllerParams(CP)
self.lfa_enabled = False
self.prev_lfa_enabled = False
self.mainEnabled = False
def update(self, cp, cp_cam):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
@@ -82,7 +86,7 @@ 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_from_lamp(
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = 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"]
@@ -92,7 +96,7 @@ class CarState(CarStateBase):
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 and self.mainEnabled
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
else:
@@ -148,8 +152,18 @@ class CarState(CarStateBase):
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.prev_main_buttons = self.main_buttons[-1]
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
if self.CP.openpilotLongitudinalControl:
if self.prev_main_buttons != 1:
if self.main_buttons[-1] == 1:
self.mainEnabled = not self.mainEnabled
ret.cruiseState.available = ret.cruiseState.available and self.mainEnabled
self.prev_mads_enabled = self.mads_enabled
self.prev_lfa_enabled = self.lfa_enabled
if self.CP.flags & HyundaiFlags.SP_CAN_LFA_BTN:
self.lfa_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
return ret
@@ -194,8 +208,8 @@ class CarState(CarStateBase):
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
cp.vl["BLINKERS"]["RIGHT_LAMP"])
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp(
50, cp.vl["BLINKERS"]["LEFT_LAMP"], cp.vl["BLINKERS"]["RIGHT_LAMP"])
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
@@ -216,8 +230,12 @@ class CarState(CarStateBase):
cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.prev_main_buttons = self.main_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"])
self.prev_mads_enabled = self.mads_enabled
self.prev_lfa_enabled = self.lfa_enabled
self.lfa_enabled = cp.vl[cruise_btn_msg]["LFA_BTN"]
self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
@@ -363,6 +381,10 @@ class CarState(CarStateBase):
signals.append(("CF_Lvr_Gear", "LVR12"))
checks.append(("LVR12", 100))
if CP.flags & HyundaiFlags.SP_CAN_LFA_BTN:
signals.append(("LFA_Pressed", "BCM_PO_11"))
checks.append(("BCM_PO_11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
@@ -448,6 +470,7 @@ class CarState(CarStateBase):
("CRUISE_BUTTONS", cruise_btn_msg),
("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg),
("DISTANCE_UNIT", "CRUISE_BUTTONS_ALT"),
("LFA_BTN", cruise_btn_msg),
("LEFT_LAMP", "BLINKERS"),
("RIGHT_LAMP", "BLINKERS"),
selfdrive/car/hyundai/hyundaican.py
+9 -7
View File
@@ -6,7 +6,8 @@ hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def create_lkas11(packer, frame, car_fingerprint, 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,
lateral_paused, blinking_icon):
values = lkas11
values["CF_Lkas_LdwsSysState"] = sys_state
values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0
@@ -31,7 +32,8 @@ def create_lkas11(packer, frame, car_fingerprint, 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"] = 2 if steer_req else 2 if blinking_icon else 1 if\
lateral_paused else 1
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
@@ -48,7 +50,7 @@ def create_lkas11(packer, frame, car_fingerprint, 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"] = 3 if steer_req else 1
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
# these have no effect
@@ -87,20 +89,20 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint):
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon, hda_set_speed=0):
values = {
"LFA_Icon_State": 2 if enabled else 0,
"LFA_Icon_State": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0,
"HDA_Active": 1 if hda_set_speed else 0,
"HDA_Icon_State": 2 if hda_set_speed else 0,
"HDA_VSetReq": hda_set_speed,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, main_enabled):
commands = []
scc11_values = {
"MainMode_ACC": 1,
"MainMode_ACC": 1 if main_enabled else 0,
"TauGapSet": 4,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
selfdrive/car/hyundai/hyundaicanfd.py
+4 -4
View File
@@ -8,13 +8,13 @@ def get_e_can_bus(CP):
return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4
def create_steering_messages(packer, CP, enabled, lat_active, apply_steer):
def create_steering_messages(packer, CP, enabled, lat_active, apply_steer, lateral_paused, blinking_icon):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"LKA_ICON": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
@@ -56,10 +56,10 @@ def create_acc_cancel(packer, CP, cruise_info_copy):
})
return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values)
def create_lfahda_cluster(packer, CP, enabled):
def create_lfahda_cluster(packer, CP, enabled, lat_active, lateral_paused, blinking_icon):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
"LFA_ICON": 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values)
selfdrive/car/hyundai/interface.py
+45 -5
View File
@@ -11,6 +11,7 @@ from selfdrive.car.disable_ecu import disable_ecu
Ecu = car.CarParams.Ecu
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
@@ -267,6 +268,10 @@ class CarInterface(CarInterfaceBase):
if candidate in CAMERA_SCC_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
if 0x391 in fingerprint[0]:
ret.flags |= HyundaiFlags.SP_CAN_LFA_BTN.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
if candidate in HYBRID_CAR:
@@ -301,26 +306,61 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons:
buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)]
buttonEvents = []
if self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons:
buttonEvents.append(create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT))
# Handle CF_Clu_CruiseSwState changing buttons mid-press
if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0:
buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT))
ret.buttonEvents = buttonEvents
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
buttonEvents, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.prev_lfa_enabled != 1 and self.CS.lfa_enabled == 1:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise:
if any(b.type == ButtonType.cancel for b in buttonEvents):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# 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 ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.manumatic],
pcm_enable=False, allow_enable=allow_enable)
events, ret = self.create_sp_events(self.CS, ret, events, main_enabled=True, allow_enable=allow_enable)
# 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.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
if self.low_speed_alert and self.CS.madsEnabled:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
selfdrive/car/hyundai/values.py
+2
View File
@@ -61,6 +61,8 @@ class HyundaiFlags(IntFlag):
ENABLE_BLINKERS = 32
SP_CAN_LFA_BTN = 64
class CAR:
# Hyundai
selfdrive/car/interfaces.py
+175 -15
View File
@@ -9,9 +9,11 @@ from common.basedir import BASEDIR
from common.conversions import Conversions as CV
from common.kalman.simple_kalman import KF1D
from common.numpy_fast import clip, interp
from common.params import Params
from common.realtime import DT_CTRL
from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone
from selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, V_CRUISE_MAX, apply_center_deadzone
from selfdrive.controls.lib.events import Events
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -83,6 +85,17 @@ class CarInterfaceBase(ABC):
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
self.param_s = Params()
self.disengage_on_accelerator = self.param_s.get_bool("DisengageOnAccelerator")
self.enable_mads = self.param_s.get_bool("EnableMads")
self.mads_disengage_lateral_on_brake = self.param_s.get_bool("DisengageLateralOnBrake")
self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
self.gear_warning = 0
self.cruise_cancelled_btn = True
self.acc_mads_combo = self.param_s.get_bool("AccMadsCombo")
self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause")
self.prev_acc_mads_combo = False
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return ACCEL_MIN, ACCEL_MAX
@@ -242,13 +255,19 @@ class CarInterfaceBase(ABC):
if cs_out.doorOpen:
events.add(EventName.doorOpen)
if cs_out.seatbeltUnlatched:
if cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park:
events.add(EventName.seatbeltNotLatched)
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.drive and cs_out.gearShifter not in extra_gears and not \
(cs_out.gearShifter == GearShifter.unknown and self.gear_warning < int(0.5/DT_CTRL)):
if cs_out.vEgo < 5:
events.add(EventName.silentWrongGear)
else:
events.add(EventName.wrongGear)
if cs_out.gearShifter == GearShifter.reverse:
events.add(EventName.reverseGear)
if cs_out.vEgo < 5:
events.add(EventName.spReverseGear)
else:
events.add(EventName.reverseGear)
if not cs_out.cruiseState.available:
events.add(EventName.wrongCarMode)
if cs_out.espDisabled:
@@ -262,7 +281,12 @@ class CarInterfaceBase(ABC):
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if cs_out.madsEnabled:
cs_out.disengageByBrake = True
if cs_out.cruiseState.enabled:
events.add(EventName.brakeHold)
else:
events.add(EventName.silentBrakeHold)
if cs_out.parkingBrake:
events.add(EventName.parkBrake)
if cs_out.accFaulted:
@@ -270,14 +294,16 @@ class CarInterfaceBase(ABC):
if cs_out.steeringPressed:
events.add(EventName.steerOverride)
self.gear_warning = self.gear_warning + 1 if cs_out.gearShifter == GearShifter.unknown else 0
# Handle button presses
for b in cs_out.buttonEvents:
# Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
events.add(EventName.buttonEnable)
# Disable on rising and falling edge of cancel for both stock and OP long
if b.type == ButtonType.cancel:
events.add(EventName.buttonCancel)
#for b in cs_out.buttonEvents:
# # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
# if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
# events.add(EventName.buttonEnable)
# # Disable on rising and falling edge of cancel for both stock and OP long
# if b.type == ButtonType.cancel:
# events.add(EventName.buttonCancel)
# Handle permanent and temporary steering faults
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
@@ -293,6 +319,12 @@ class CarInterfaceBase(ABC):
if cs_out.steerFaultPermanent:
events.add(EventName.steerUnavailable)
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
if (cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator) or \
(cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill) and not self.mads_ndlob):
if cs_out.madsEnabled:
cs_out.disengageByBrake = True
# we engage when pcm is active (rising edge)
# enabling can optionally be blocked by the car interface
if pcm_enable:
@@ -303,6 +335,128 @@ class CarInterfaceBase(ABC):
return events
@staticmethod
def sp_v_cruise_initialized(v_cruise):
return v_cruise != V_CRUISE_INITIAL
def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled):
if self.acc_mads_combo:
if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled):
mads_enabled = True
self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled)
return mads_enabled
def get_sp_v_cruise_non_pcm_state(self, cruiseState_available, acc_enabled, button_events, vCruise,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
if cruiseState_available:
for b in button_events:
if not self.CP.pcmCruise:
if b.type in enable_buttons and not b.pressed:
acc_enabled = True
if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and not self.sp_v_cruise_initialized(vCruise):
acc_enabled = False
else:
acc_enabled = False
return acc_enabled, button_events
def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False):
mads_enabled = False if not self.enable_mads else mads_enabled
return mads_enabled, acc_enabled
def get_sp_pedal_disengage(self, brake_pressed, standstill):
return brake_pressed and (not self.CS.out.brakePressed or not standstill)
def get_sp_common_state(self, cs_out, CS, controls_cancel, gear_allowed=True):
if self.CP.pcmCruise:
if not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled:
CS.madsEnabled, cs_out.cruiseState.enabled = self.get_sp_cancel_cruise_state(CS.madsEnabled)
CS.accEnabled = False if controls_cancel else CS.accEnabled
cs_out.cruiseState.enabled = cs_out.cruiseState.enabled if self.CP.pcmCruise else CS.accEnabled
if not self.enable_mads:
if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled:
CS.madsEnabled = True
elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled:
CS.madsEnabled = False
cs_out.belowLaneChangeSpeed = cs_out.vEgo < LANE_CHANGE_SPEED_MIN and self.below_speed_pause
if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \
(cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park):
gear_allowed = False
cs_out.latActive = gear_allowed
if not CS.control_initialized:
CS.control_initialized = True
cs_out.madsEnabled = CS.madsEnabled
cs_out.accEnabled = CS.accEnabled
return cs_out, CS
def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
enable_from_brake=False, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
CS.disengageByBrake = CS.disengageByBrake or cs_out.disengageByBrake
if CS.disengageByBrake and not cs_out.brakePressed and not cs_out.brakeHoldActive and not cs_out.parkingBrake and cs_out.madsEnabled:
enable_pressed = True
enable_from_brake = True
if not cs_out.brakePressed and not cs_out.brakeHoldActive and not cs_out.parkingBrake:
CS.disengageByBrake = False
cs_out.disengageByBrake = False
for b in cs_out.buttonEvents:
# Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
if not self.CP.pcmCruise:
if b.type in enable_buttons and not b.pressed:
enable_pressed = True
# Disable on rising and falling edge of cancel for both stock and OP long
if b.type == ButtonType.cancel:
if not cs_out.madsEnabled:
events.add(EventName.buttonCancel)
elif not self.cruise_cancelled_btn:
self.cruise_cancelled_btn = True
events.add(EventName.manualLongitudinalRequired)
# do disable on MADS button if ACC is disabled
if b.type == ButtonType.altButton1 and b.pressed:
if not cs_out.madsEnabled: # disabled MADS
if not cs_out.cruiseState.enabled:
events.add(EventName.buttonCancel)
else:
events.add(EventName.manualSteeringRequired)
else: # enabled MADS
if not cs_out.cruiseState.enabled:
enable_pressed = True
if self.CP.pcmCruise:
# do disable on button down
if main_enabled:
if any(CS.main_buttons) and not cs_out.cruiseState.enabled:
if not cs_out.madsEnabled:
events.add(EventName.buttonCancel)
# do enable on both accel and decel buttons
if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled and allow_enable:
enable_pressed = True
elif not cs_out.cruiseState.enabled:
if not cs_out.madsEnabled:
events.add(EventName.buttonCancel)
elif not self.enable_mads:
cs_out.madsEnabled = False
if enable_pressed:
if enable_from_brake:
events.add(EventName.silentButtonEnable)
else:
events.add(EventName.buttonEnable)
if cs_out.cruiseState.enabled:
self.cruise_cancelled_btn = False
return events, cs_out
class RadarInterfaceBase(ABC):
def __init__(self, CP):
@@ -334,6 +488,13 @@ class CarStateBase(ABC):
self.cluster_speed_hyst_gap = 0.0
self.cluster_min_speed = 0.0 # min speed before dropping to 0
self.accEnabled = False
self.madsEnabled = False
self.disengageByBrake = False
self.mads_enabled = False
self.prev_mads_enabled = False
self.control_initialized = False
# Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
# R = 0.3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
@@ -429,7 +590,6 @@ class CarStateBase(ABC):
def get_loopback_can_parser(CP):
return None
# interface-specific helpers
def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str, Any]:
selfdrive/car/mazda/carstate.py
+11 -2
View File
@@ -18,9 +18,16 @@ class CarState(CarStateBase):
self.lkas_allowed_speed = False
self.lkas_disabled = False
self.lkas_enabled = False
self.prev_lkas_enabled = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
self.prev_lkas_enabled = self.lkas_enabled
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["FL"],
cp.vl["WHEEL_SPEEDS"]["FR"],
@@ -34,14 +41,16 @@ class CarState(CarStateBase):
speed_kph = cp.vl["ENGINE_DATA"]["SPEED"]
ret.standstill = speed_kph < .1
self.lkas_enabled = not self.lkas_disabled
can_gear = int(cp.vl["GEAR"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"])
ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1
ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"]
ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"]
selfdrive/car/mazda/interface.py
+45 -5
View File
@@ -7,6 +7,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@@ -57,12 +58,51 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# events
events = self.create_common_events(ret)
buttonEvents = []
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
elif self.CS.low_speed_alert:
# CANCEL
if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.cancel
buttonEvents.append(be)
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.prev_lkas_enabled != self.CS.lkas_enabled:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
#if self.CS.lkas_disabled:
# events.add(EventName.lkasDisabled)
if self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
selfdrive/car/nissan/carcontroller.py
+20 -2
View File
@@ -1,4 +1,5 @@
from cereal import car
from common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_angle_limits
from selfdrive.car.nissan import nissancan
@@ -18,11 +19,28 @@ class CarController:
self.packer = CANPacker(dbc_name)
self.disengage_blink = 0.
self.lat_disengage_init = False
self.lat_active_last = False
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
lateral_paused = CS.madsEnabled and not CC.latActive
if CC.latActive:
self.lat_disengage_init = False
elif self.lat_active_last:
self.lat_disengage_init = True
if not self.lat_disengage_init:
self.disengage_blink = self.frame
blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
self.lat_active_last = CC.latActive
can_sends = []
### STEER ###
@@ -63,12 +81,12 @@ class CarController:
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd))
can_sends.append(nissancan.create_steering_control(
self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque))
self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque))
if lkas_hud_msg and lkas_hud_info_msg:
if self.frame % 2 == 0:
can_sends.append(nissancan.create_lkas_hud_msg(
self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
self.packer, lkas_hud_msg, CC.latActive, blinking_icon, lateral_paused, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
if self.frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(
selfdrive/car/nissan/carstate.py
+4 -2
View File
@@ -23,6 +23,8 @@ class CarState(CarStateBase):
def update(self, cp, cp_adas, cp_cam):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
@@ -88,8 +90,8 @@ class CarState(CarStateBase):
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])
ret.leftBlinker = ret.leftBlinkerOn = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = ret.rightBlinkerOn = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])
ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"],
cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"],
selfdrive/car/nissan/interface.py
+42 -4
View File
@@ -4,6 +4,9 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.nissan.values import CAR
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@@ -43,11 +46,46 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
buttonEvents = []
be = car.CarState.ButtonEvent.new_message()
be.type = car.CarState.ButtonEvent.Type.accelCruise
buttonEvents.append(be)
#be = car.CarState.ButtonEvent.new_message()
#be.type = car.CarState.ButtonEvent.Type.accelCruise
#buttonEvents.append(be)
events = self.create_common_events(ret)
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# CANCEL
if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.cancel
buttonEvents.append(be)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
selfdrive/car/nissan/nissancan.py
+4 -4
View File
@@ -45,15 +45,15 @@ def create_cancel_msg(packer, cancel_msg, cruise_cancel):
return packer.make_can_msg("CANCEL_MSG", 2, values)
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart):
def create_lkas_hud_msg(packer, lkas_hud_msg, lat_active, blinking_icon, lateral_paused, left_line, right_line, left_lane_depart, right_lane_depart):
values = lkas_hud_msg
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 3 if blinking_icon else 1 if lateral_paused else 0,
values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0
return packer.make_can_msg("PROPILOT_HUD", 0, values)
selfdrive/car/subaru/carcontroller.py
+1 -1
View File
@@ -80,7 +80,7 @@ class CarController:
self.es_dashstatus_cnt = CS.es_dashstatus_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, CC.enabled, hud_control.visualAlert,
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.latActive, CS.madsEnabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"]
selfdrive/car/subaru/carstate.py
+18 -2
View File
@@ -13,9 +13,15 @@ class CarState(CarStateBase):
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.shifter_values = can_define.dv["Transmission"]["Gear"]
self.lkas_enabled = None
self.prev_lkas_enabled = None
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
self.prev_lkas_enabled = self.lkas_enabled
ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255.
ret.gasPressed = ret.gas > 1e-5
if self.car_fingerprint in PREGLOBAL_CARS:
@@ -35,9 +41,14 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw == 0
if self.car_fingerprint not in PREGLOBAL_CARS:
self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
if self.prev_lkas_enabled is None:
self.prev_lkas_enabled = self.lkas_enabled
# continuous blinker signals for assisted lane change
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
cp.vl["Dashlights"]["RIGHT_BLINKER"])
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = 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)
@@ -62,6 +73,11 @@ class CarState(CarStateBase):
(self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1):
ret.cruiseState.speed *= CV.MPH_TO_KPH
self.madsEnabled, ret.cruiseState.enabled = self.update_sp_state(ret.cruiseState.enabled, self.enable_mads,
self.accEnabled, self.prev_cruiseState_enabled,
self.madsEnabled)
self.prev_brake_pressed = ret.brakePressed
ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1
ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"],
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
selfdrive/car/subaru/interface.py
+51 -1
View File
@@ -5,6 +5,10 @@ from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@@ -108,7 +112,53 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.events = self.create_common_events(ret).to_msg()
buttonEvents = []
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.car_fingerprint not in PREGLOBAL_CARS:
if self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.lkas_enabled != 2 and not (self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled == 1):
self.CS.madsEnabled = not self.CS.madsEnabled
elif self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled != 1:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if self.CS.out.cruiseState.enabled: # CANCEL
if not ret.cruiseState.enabled:
if not self.enable_mads:
self.CS.madsEnabled = False
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# CANCEL
if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.cancel
buttonEvents.append(be)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
ret.events = events.to_msg()
return ret
selfdrive/car/subaru/subarucan.py
+7 -2
View File
@@ -21,7 +21,7 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
values["Cruise_Cancel"] = 1
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def create_es_lkas(packer, es_lkas_msg, lat_active, mads_enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
values = copy.copy(es_lkas_msg)
@@ -56,12 +56,17 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_
elif right_lane_depart:
values["LKAS_Alert"] = 11 # Right lane departure dash alert
if enabled:
if lat_active:
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
values["LKAS_Dash_State"] = 2 # Green enabled indicator
elif mads_enabled and not lat_active:
values["LKAS_Dash_State"] = 1 # White ready indicator
else:
values["LKAS_Dash_State"] = 0 # LKAS Not enabled
values["LKAS_Left_Line_Enable"] = 1 if lat_active else 0
values["LKAS_Right_Line_Enable"] = 1 if lat_active else 0
values["LKAS_Left_Line_Visible"] = int(left_line)
values["LKAS_Right_Line_Visible"] = int(right_line)
selfdrive/car/toyota/carcontroller.py
+2 -2
View File
@@ -78,7 +78,7 @@ class CarController:
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not CC.enabled and CS.pcm_acc_status:
if not (CC.enabled and CS.out.cruiseState.enabled) and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
@@ -146,7 +146,7 @@ class CarController:
if self.frame % 100 == 0 or send_ui:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud, CS.madsEnabled))
if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
selfdrive/car/toyota/carstate.py
+28 -2
View File
@@ -30,9 +30,19 @@ class CarState(CarStateBase):
self.acc_type = 1
self.lkas_hud = {}
self.lkas_enabled = None
self.prev_lkas_enabled = None
self.lta_status = False
self.prev_lta_status = False
self.lta_status_active = False
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
self.prev_lkas_enabled = self.lkas_enabled
self.prev_lta_status = self.lta_status
ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
@@ -61,6 +71,20 @@ class CarState(CarStateBase):
ret.standstill = ret.vEgoRaw == 0
if self.CP.carFingerprint != CAR.PRIUS_V:
self.lta_status = cp_cam.vl["LKAS_HUD"]["SET_ME_X02"]
if ((self.prev_lta_status == 16 and self.lta_status == 0) or
(self.prev_lta_status == 0 and self.lta_status == 16)) and not self.lta_status_active:
self.lta_status_active = True
if self.prev_lta_status is None:
self.prev_lta_status = self.lta_status
if self.lta_status_active:
self.lkas_enabled = self.lta_status
elif self.CP.carFingerprint != CAR.PRIUS_V:
self.lkas_enabled = cp_cam.vl["LKAS_HUD"]["LKAS_STATUS"]
if self.prev_lkas_enabled is None:
self.prev_lkas_enabled = self.lkas_enabled
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
@@ -81,8 +105,8 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
@@ -267,6 +291,8 @@ class CarState(CarStateBase):
("LANE_SWAY_WARNING", "LKAS_HUD"),
("LANE_SWAY_SENSITIVITY", "LKAS_HUD"),
("LANE_SWAY_TOGGLE", "LKAS_HUD"),
("LKAS_STATUS", "LKAS_HUD"),
("SET_ME_X02", "LKAS_HUD"),
]
checks += [
("LKAS_HUD", 1),
selfdrive/car/toyota/interface.py
+49 -2
View File
@@ -3,10 +3,12 @@ from cereal import car
from common.conversions import Conversions as CV
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
class CarInterface(CarInterfaceBase):
@@ -240,8 +242,53 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
buttonEvents = []
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
if self.CS.lta_status_active:
if (self.CS.prev_lkas_enabled == 16 and self.CS.lkas_enabled == 0) or \
(self.CS.prev_lkas_enabled == 0 and self.CS.lkas_enabled == 16):
self.CS.madsEnabled = not self.CS.madsEnabled
else:
if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \
(self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.CS.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# CANCEL
if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.cancel
buttonEvents.append(be)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret)
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
if self.CP.openpilotLongitudinalControl:
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
selfdrive/car/toyota/toyotacan.py
+8 -6
View File
@@ -66,18 +66,20 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values)
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, lat_active, stock_lkas_hud,
mads_enabled):
lateral_paused = mads_enabled and not lat_active
values = {
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS": 1 if enabled else 0,
"LDA_ALERT": steer if mads_enabled else 0,
"RIGHT_LINE": 0 if not mads_enabled else 2 if lateral_paused else 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 0 if not mads_enabled else 2 if lateral_paused else 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS": 1 if lat_active else 0,
"LKAS_STATUS": 2 if mads_enabled else 1 if lateral_paused else 0,
# static signals
"SET_ME_X02": 2,
"SET_ME_X01": 1,
"LKAS_STATUS": 1,
"REPEATED_BEEPS": 0,
"LANE_SWAY_FLD": 7,
"LANE_SWAY_BUZZER": 0,
selfdrive/car/volkswagen/carcontroller.py
+1 -1
View File
@@ -85,7 +85,7 @@ class CarController:
hud_alert = 0
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, CC.latActive,
CS.out.steeringPressed, hud_alert, hud_control))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
selfdrive/car/volkswagen/carstate.py
+10 -4
View File
@@ -34,6 +34,9 @@ class CarState(CarStateBase):
return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type)
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
# Update vehicle speed and acceleration from ABS wheel speeds.
ret.wheelSpeeds = self.get_wheel_speeds(
pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"],
@@ -134,8 +137,8 @@ class CarState(CarStateBase):
ret.cruiseState.speed = 0
# Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"])
ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
ret.leftBlinker = ret.leftBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"])
ret.rightBlinker = ret.rightBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
self.gra_stock_values = pt_cp.vl["GRA_ACC_01"]
@@ -149,6 +152,9 @@ class CarState(CarStateBase):
def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
ret = car.CarState.new_message()
self.prev_mads_enabled = self.mads_enabled
# Update vehicle speed and acceleration from ABS wheel speeds.
ret.wheelSpeeds = self.get_wheel_speeds(
pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"],
@@ -239,8 +245,8 @@ class CarState(CarStateBase):
ret.cruiseState.speed = 0
# Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"],
pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])
ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"],
pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
self.gra_stock_values = pt_cp.vl["GRA_Neu"]
selfdrive/car/volkswagen/interface.py
+31 -2
View File
@@ -217,16 +217,45 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
ret.buttonEvents, c.vCruise,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if any(b.type == ButtonType.cancel for b in buttonEvents):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
if self.get_sp_pedal_disengage(ret.brakePressed, ret.standstill):
self.CS.madsEnabled = False if not self.enable_mads else self.CS.madsEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0:
if ret.gasPressed and not ret.cruiseState.enabled:
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, c.cruiseControl.cancel)
# MADS BUTTON
if self.CS.out.madsEnabled != self.CS.madsEnabled:
be = car.CarState.ButtonEvent.new_message()
be.pressed = True
be.type = ButtonType.altButton1
ret.buttonEvents.append(be)
events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
pcm_enable=not self.CS.CP.openpilotLongitudinalControl,
pcm_enable=False,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
events, ret = self.create_sp_events(self.CS, ret, events,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
self.low_speed_alert = True
elif ret.vEgo > (self.CP.minSteerSpeed + 2.):
self.low_speed_alert = False
if self.low_speed_alert:
if self.low_speed_alert and self.CS.madsEnabled:
events.add(EventName.belowSteerSpeed)
if self.CS.CP.openpilotLongitudinalControl:
selfdrive/car/volkswagen/mqbcan.py
+3 -3
View File
@@ -13,12 +13,12 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
return packer.make_can_msg("HCA_01", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, steering_pressed, hud_alert, hud_control):
values = ldw_stock_values.copy()
values.update({
"LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Texte": hud_alert,
selfdrive/car/volkswagen/pqcan.py
+3 -3
View File
@@ -9,12 +9,12 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
return packer.make_can_msg("HCA_1", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, lat_active, steering_pressed, hud_alert, hud_control):
values = ldw_stock_values.copy()
values.update({
"LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Textbits": hud_alert,
selfdrive/controls/controlsd.py
+51 -17
View File
@@ -108,9 +108,20 @@ class Controls:
# set alternative experiences from parameters
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
self.enable_mads = self.params.get_bool("EnableMads")
self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
self.mads_dlob = self.enable_mads and self.mads_disengage_lateral_on_brake
self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
if self.enable_mads and self.disengage_on_accelerator:
self.params.put_bool("DisengageOnAccelerator", False)
self.disengage_on_accelerator = False
self.CP.alternativeExperience = 0
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
if self.mads_dlob:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
elif self.mads_ndlob:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE
if self.CP.dashcamOnly and self.params.get_bool("DashcamOverride"):
self.CP.dashcamOnly = False
@@ -166,10 +177,12 @@ class Controls:
self.initialized = False
self.state = State.disabled
self.enabled = False
self.enabled_long = False
self.active = False
self.can_rcv_timeout = False
self.soft_disable_timer = 0
self.mismatch_counter = 0
self.mismatch_counter_long = 0
self.cruise_mismatch_counter = 0
self.can_rcv_timeout_counter = 0
self.last_blinker_frame = 0
@@ -245,12 +258,15 @@ class Controls:
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
if CS.cruiseState.enabled:
self.events.add(EventName.pedalPressed)
else:
self.events.add(EventName.silentPedalPressed)
if CS.brakePressed and CS.standstill:
if CS.brakePressed and CS.standstill and CS.cruiseState.enabled:
self.events.add(EventName.preEnableStandstill)
if CS.gasPressed:
if CS.gasPressed and CS.cruiseState.enabled:
self.events.add(EventName.gasPressedOverride)
if not self.CP.notCar:
@@ -317,6 +333,8 @@ class Controls:
if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if self.mismatch_counter_long >= 200:
self.events.add(EventName.controlsMismatchLong)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
@@ -383,7 +401,7 @@ class Controls:
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
cruise_mismatch = CS.cruiseState.enabled and not self.enabled
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
@@ -451,11 +469,16 @@ class Controls:
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.enabled_long:
self.mismatch_counter_long = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
if self.enabled_long and any(not ps.controlsAllowedLong for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter_long += 1
self.distance_traveled += CS.vEgo * DT_CTRL
@@ -464,7 +487,7 @@ class Controls:
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
@@ -486,6 +509,12 @@ class Controls:
else:
# ENABLED
if self.state == State.enabled:
if CS.cruiseState.enabled and not self.CS_prev.cruiseState.enabled:
self.v_cruise_helper.initialize_v_cruise(CS)
# Block resume if cruise never previously enabled
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
self.current_alert_types.append(ET.NO_ENTRY)
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
@@ -539,10 +568,12 @@ class Controls:
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_helper.initialize_v_cruise(CS)
if CS.cruiseState.enabled:
self.v_cruise_helper.initialize_v_cruise(CS)
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
self.enabled_long = self.enabled and CS.cruiseState.enabled
self.active = self.state in ACTIVE_STATES
if self.active:
self.current_alert_types.append(ET.WARNING)
@@ -570,9 +601,11 @@ class Controls:
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
CC.latActive = (self.active or self.mads_ndlob) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode) and CS.madsEnabled and (not CS.brakePressed or self.mads_ndlob) and \
(not CS.belowLaneChangeSpeed or (not (((self.sm.frame - self.last_blinker_frame) * DT_CTRL) < 1.0) and
not (CS.leftBlinker or CS.rightBlinker))) and CS.latActive
CC.longActive = self.enabled_long and not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and not self.events.any(ET.OVERRIDE_LONGITUDINAL)
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
@@ -624,14 +657,14 @@ class Controls:
lac_log.saturated = abs(actuators.steer) >= 0.9
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
if lac_log.active and not CS.steeringPressed and self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode and CS.madsEnabled:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
good_speed = CS.vEgo > 5
max_torque = abs(self.last_actuators.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
self.events.add(EventName.steerSaturated)
elif lac_log.active and lac_log.saturated:
elif lac_log.active and lac_log.saturated and CS.madsEnabled:
dpath_points = lat_plan.dPathPoints
if len(dpath_points):
# Check if we deviated from the path
@@ -671,18 +704,19 @@ class Controls:
if len(angular_rate_value) > 2:
CC.angularVelocity = angular_rate_value
CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
CC.cruiseControl.override = self.enabled_long and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled_long or (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)))
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds):
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
CC.vCruise = float(self.v_cruise_helper.v_cruise_kph)
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.speedVisible = self.enabled_long
hudControl.lanesVisible = self.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
@@ -714,7 +748,7 @@ class Controls:
clear_event_types = set()
if ET.WARNING not in self.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
if self.enabled and (self.CP.pcmCruise or (not self.CP.pcmCruise and self.v_cruise_helper.v_cruise_initialized)):
clear_event_types.add(ET.NO_ENTRY)
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer])
@@ -758,8 +792,8 @@ class Controls:
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled)
controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled)
controlsState.curvature = curvature
controlsState.desiredCurvature = self.desired_curvature
controlsState.desiredCurvatureRate = self.desired_curvature_rate
selfdrive/controls/lib/desire_helper.py
+2 -2
View File
@@ -45,12 +45,12 @@ class DesireHelper:
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
if not carstate.madsEnabled or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
# LaneChangeState.off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed and not carstate.brakePressed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
selfdrive/controls/lib/events.py
+70
View File
@@ -542,6 +542,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.manualSteeringRequired: {
ET.WARNING: Alert(
"Automatic Lane Centering is OFF",
"Manual Steering Required",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
},
EventName.manualLongitudinalRequired: {
ET.WARNING: Alert(
"Smart/Adaptive Cruise Control is OFF",
"Manual Gas/Brakes Required",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
},
EventName.steerSaturated: {
ET.WARNING: Alert(
"Take Control",
@@ -590,6 +606,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.silentButtonEnable: {
ET.ENABLE: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.MID, VisualAlert.none, AudibleAlert.none, .2, 0., 0.),
},
EventName.pcmDisable: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
@@ -604,6 +628,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
},
EventName.silentBrakeHold: {
ET.USER_DISABLE: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.MID, VisualAlert.none, AudibleAlert.none, .2, 0., 0.),
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
},
EventName.parkBrake: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
@@ -615,6 +648,16 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
visual_alert=VisualAlert.brakePressed),
},
EventName.silentPedalPressed: {
ET.USER_DISABLE: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.MID, VisualAlert.none, AudibleAlert.none, .2),
ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt",
visual_alert=VisualAlert.brakePressed),
},
EventName.preEnableStandstill: {
ET.PRE_ENABLE: Alert(
"Release Brake to Engage",
@@ -701,6 +744,19 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("Gear not D"),
},
EventName.silentWrongGear: {
ET.SOFT_DISABLE: Alert(
"Gear not D",
"openpilot Unavailable",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 2., 3.),
ET.NO_ENTRY: Alert(
"Gear not D",
"openpilot Unavailable",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 2., 3.),
},
# This alert is thrown when the calibration angles are outside of the acceptable range.
# For example if the device is pointed too much to the left or the right.
# Usually this can only be solved by removing the mount from the windshield completely,
@@ -820,6 +876,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),
},
EventName.controlsMismatchLong: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch\nLongitudinal"),
ET.NO_ENTRY: NoEntryAlert("Controls Mismatch\nLongitudinal"),
},
EventName.roadCameraError: {
ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road",
duration=1.,
@@ -892,6 +953,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
},
EventName.spReverseGear: {
ET.PERMANENT: Alert(
"Reverse\nGear",
"",
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
},
# On cars that use stock ACC the car can decide to cancel ACC for various reasons.
# When this happens we can no long control the car so the user needs to be warned immediately.
EventName.cruiseDisabled: {
selfdrive/manager/manager.py
+6 -1
View File
@@ -36,11 +36,16 @@ def manager_init() -> None:
default_params: List[Tuple[str, Union[str, bytes]]] = [
("CompletedTrainingVersion", "0"),
("DisengageOnAccelerator", "1"),
("DisengageOnAccelerator", "0"),
("GsmMetered", "1"),
("HasAcceptedTerms", "0"),
("LanguageSetting", "main_en"),
("OpenpilotEnabledToggle", "1"),
("AccMadsCombo", "1"),
("BelowSpeedPause", "1"),
("DisengageLateralOnBrake", "1"),
("EnableMads", "1"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
selfdrive/ui/ui.cc
+4 -1
View File
@@ -178,6 +178,8 @@ void ui_update_params(UIState *s) {
void UIState::updateStatus() {
if (scene.started && sm->updated("controlsState")) {
auto controls_state = (*sm)["controlsState"].getControlsState();
auto car_control = (*sm)["carControl"].getCarControl();
auto car_state = (*sm)["carState"].getCarState();
auto alert_status = controls_state.getAlertStatus();
auto state = controls_state.getState();
if (alert_status == cereal::ControlsState::AlertStatus::USER_PROMPT) {
@@ -187,7 +189,7 @@ void UIState::updateStatus() {
} else if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) {
status = STATUS_OVERRIDE;
} else {
status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
status = car_state.getMadsEnabled() ? car_control.getLongActive() ? STATUS_ENGAGED : STATUS_MADS : STATUS_DISENGAGED;
}
}
@@ -215,6 +217,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements",
"carControl",
});
Params params;
selfdrive/ui/ui.h
+3 -1
View File
@@ -73,6 +73,7 @@ typedef enum UIStatus {
STATUS_DISENGAGED,
STATUS_OVERRIDE,
STATUS_ENGAGED,
STATUS_MADS,
STATUS_WARNING,
STATUS_ALERT,
} UIStatus;
@@ -80,7 +81,8 @@ typedef enum UIStatus {
const QColor bg_colors [] = {
[STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
[STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1),
[STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
[STATUS_ENGAGED] = QColor(0x00, 0xc8, 0x00, 0xf1),
[STATUS_MADS] = QColor(0x00, 0xc8, 0xc8, 0xf1),
[STATUS_WARNING] = QColor(0xDA, 0x6F, 0x25, 0xf1),
[STATUS_ALERT] = QColor(0xC9, 0x22, 0x31, 0xf1),
};
system/version.py
+3 -3
View File
@@ -7,8 +7,8 @@ from functools import lru_cache
from common.basedir import BASEDIR
from system.swaglog import cloudlog
RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3']
TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging']
RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'prod-c3']
TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging', 'test-c3']
training_version: bytes = b"0.2.0"
terms_version: bytes = b"2"
@@ -90,7 +90,7 @@ def is_comma_remote() -> bool:
if origin is None:
return False
return origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai')
return origin.startswith('git@github.com:sunnyhaibin') or origin.startswith('https://github.com/sunnyhaibin')
@cache