diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 10fc83eb0..eb932b484 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -63,7 +63,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { type @0 :Type; artifact @1 :Artifact; # Main artifact metadata @2 :Artifact; # Metadata artifact - + enum Type { supercombo @0; navigation @1; @@ -144,6 +144,7 @@ struct OnroadEventSP @0xda96579883444c35 { hyundaiRadarTracksConfirmed @13; experimentalModeSwitched @14; wrongCarModeAlertOnly @15; + pedalPressedAlertOnly @16; } } @@ -176,14 +177,14 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { lastError @4 :Text; currentBackup @5 :BackupInfo; backupHistory @6 :List(BackupInfo); - + enum Status { idle @0; inProgress @1; completed @2; failed @3; } - + struct Version { major @0 :UInt16; minor @1 :UInt16; @@ -191,13 +192,13 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { build @3 :UInt16; branch @4 :Text; } - + struct MetadataEntry { key @0 :Text; value @1 :Text; tags @2 :List(Text); } - + struct BackupInfo { deviceId @0 :Text; version @1 :UInt32; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6f9c13b4a..164db8967 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import math from typing import SupportsFloat -from cereal import car, log, custom +from cereal import car, log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params @@ -22,6 +22,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T from openpilot.common.pt2 import PT2Filter from openpilot.common.realtime import DT_CTRL +from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt + State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -29,7 +31,7 @@ LaneChangeDirection = log.LaneChangeDirection ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) -class Controls: +class Controls(ControlsExt): def __init__(self) -> None: self.params = Params() self.param_counter = 0 @@ -37,17 +39,16 @@ class Controls: self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) cloudlog.info("controlsd got CarParams") - cloudlog.info("controlsd is waiting for CarParamsSP") - self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP) - cloudlog.info("controlsd got CarParamsSP") + # Initialize sunnypilot controlsd extension + ControlsExt.__init__(self, self.params) self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ['selfdriveStateSP'], + 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext, poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ['carControlSP']) + self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext) self.steer_limited_by_controls = False self.curvature = 0.0 @@ -116,11 +117,8 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill - ss_sp = self.sm['selfdriveStateSP'] - if ss_sp.mads.available: - _lat_active = ss_sp.mads.active - else: - _lat_active = self.sm['selfdriveState'].active + # Get which state to use for active lateral control + _lat_active = self.get_lat_active(self.sm) CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.CP.steerAtStandstill) @@ -168,12 +166,9 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) - CC_SP = custom.CarControlSP.new_message() - CC_SP.mads = ss_sp.mads + return CC, lac_log - return CC, CC_SP, lac_log - - def publish(self, CC, CC_SP, lac_log): + def publish(self, CC, lac_log): CS = self.sm['carState'] CC.rollDEPRECATED = self.roll # for lateral iso limit calculation @@ -252,18 +247,13 @@ class Controls: cc_send.carControl = CC self.pm.send('carControl', cc_send) - # carControlSP - cc_sp_send = messaging.new_message('carControlSP') - cc_sp_send.valid = CS.canValid - cc_sp_send.carControlSP = CC_SP - self.pm.send('carControlSP', cc_sp_send) - def run(self): rk = Ratekeeper(100, print_delay_threshold=None) while True: self.update() - CC, CC_SP, lac_log = self.state_control() - self.publish(CC, CC_SP, lac_log) + CC, lac_log = self.state_control() + self.publish(CC, lac_log) + self.run_ext(self.sm, self.pm) rk.monitor_time() diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 93f5949ea..30e294937 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -71,6 +71,13 @@ SUPPORTED_FW_VERSIONS = { b"DLhe SCC FHCUP 1.00 1.02 99110-L7000 \x01 \x102 ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + # 2022 Niro EV + b"DEev SCC F-CUP 1.00 1.00 99110-Q4600\x01\x42 ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), + b"DEev SCC F-CUP 1.00 1.00 99110-Q4600 \x07\x03\t% ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), } if __name__ == "__main__": diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 9aeb67972..d5ce9354c 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -9,6 +9,7 @@ from cereal import log, custom from opendbc.car import structs from opendbc.car.hyundai.values import HyundaiFlags +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -24,6 +25,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): + self.CP = selfdrive.CP self.params = selfdrive.params self.enabled = False @@ -35,9 +37,10 @@ class ModularAssistiveDrivingSystem: self.state_machine = StateMachine(self) self.events = self.selfdrive.events self.events_sp = self.selfdrive.events_sp + self.disengage_on_accelerator = not self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - if self.selfdrive.CP.brand == "hyundai": - if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): + if self.CP.brand == "hyundai": + if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True # read params on init @@ -50,44 +53,73 @@ class ModularAssistiveDrivingSystem: self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") + def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool: + if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator): + return True + + return False + + def should_silent_lkas_enable(self, CS: structs.CarState) -> bool: + if self.pause_lateral_on_brake_toggle and self.pedal_pressed_non_gas_pressed(CS): + return False + + if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): + return False + + return True + + def block_unified_engagement_mode(self) -> bool: + # UEM disabled + if not self.unified_engagement_mode: + return True + + if self.enabled: + return True + + if self.selfdrive.enabled and self.selfdrive.enabled_prev: + return True + + return False + + def get_wrong_car_mode(self, alert_only: bool) -> None: + if alert_only: + if self.events.has(EventName.wrongCarMode): + self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) + else: + self.events.remove(EventName.wrongCarMode) + + def transition_paused_state(self): + if self.state_machine.state != State.paused: + self.events_sp.add(EventNameSP.silentLkasDisable) + + def replace_event(self, old_event: int, new_event: int): + self.events.remove(old_event) + self.events_sp.add(new_event) + def update_events(self, CS: structs.CarState): - def update_unified_engagement_mode(): - uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev) - if (self.unified_engagement_mode and uem_blocked) or not self.unified_engagement_mode: - self.events.remove(EventName.pcmEnable) - self.events.remove(EventName.buttonEnable) - - def transition_paused_state(): - if self.state_machine.state != State.paused: - self.events_sp.add(EventNameSP.silentLkasDisable) - - def replace_event(old_event: int, new_event: int): - self.events.remove(old_event) - self.events_sp.add(new_event) - if not self.selfdrive.enabled and self.enabled: if self.events.has(EventName.doorOpen): - replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) - transition_paused_state() + self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) + self.transition_paused_state() if self.events.has(EventName.seatbeltNotLatched): - replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) - transition_paused_state() + self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) + self.transition_paused_state() if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse): - replace_event(EventName.wrongGear, EventNameSP.silentWrongGear) - transition_paused_state() + self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear) + self.transition_paused_state() if self.events.has(EventName.reverseGear): - replace_event(EventName.reverseGear, EventNameSP.silentReverseGear) - transition_paused_state() + self.replace_event(EventName.reverseGear, EventNameSP.silentReverseGear) + self.transition_paused_state() if self.events.has(EventName.brakeHold): - replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold) - transition_paused_state() + self.replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold) + self.transition_paused_state() if self.events.has(EventName.parkBrake): - replace_event(EventName.parkBrake, EventNameSP.silentParkBrake) - transition_paused_state() + self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake) + self.transition_paused_state() if self.pause_lateral_on_brake_toggle: - if CS.brakePressed: - transition_paused_state() + if self.pedal_pressed_non_gas_pressed(CS): + self.transition_paused_state() self.events.remove(EventName.preEnableStandstill) self.events.remove(EventName.belowEngageSpeed) @@ -95,8 +127,16 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.cruiseDisabled) self.events.remove(EventName.manualRestart) - if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable): - update_unified_engagement_mode() + selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable) + set_speed_btns_enable = any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents) + + # wrongCarMode alert only or actively block control + self.get_wrong_car_mode(selfdrive_enable_events or set_speed_btns_enable) + + if selfdrive_enable_events: + if self.block_unified_engagement_mode(): + self.events.remove(EventName.pcmEnable) + self.events.remove(EventName.buttonEnable) else: if self.main_enabled_toggle: if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available: @@ -120,8 +160,7 @@ class ModularAssistiveDrivingSystem: if self.selfdrive.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasDisable) - if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \ - not self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): + if self.should_silent_lkas_enable(CS): if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) @@ -129,11 +168,6 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.buttonCancel) self.events.remove(EventName.pedalPressed) self.events.remove(EventName.wrongCruiseMode) - if any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents): - if self.events.has(EventName.wrongCarMode): - replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) - else: - self.events.remove(EventName.wrongCarMode) def update(self, CS: structs.CarState): if not self.enabled_toggle: @@ -141,7 +175,7 @@ class ModularAssistiveDrivingSystem: self.update_events(CS) - if not self.selfdrive.CP.passive and self.selfdrive.initialized: + if not self.CP.passive and self.selfdrive.initialized: self.enabled, self.active = self.state_machine.update() # Copy of previous SelfdriveD states for MADS events handling diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py new file mode 100644 index 000000000..68131001a --- /dev/null +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -0,0 +1,53 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import cereal.messaging as messaging +from cereal import custom + +from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + + +class ControlsExt: + def __init__(self, params: Params): + cloudlog.info("controlsd_ext is waiting for CarParamsSP") + self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) + cloudlog.info("controlsd_ext got CarParamsSP") + + self.sm_services_ext = ['selfdriveStateSP'] + self.pm_services_ext = ['carControlSP'] + + @staticmethod + def get_lat_active(sm: messaging.SubMaster) -> bool: + ss_sp = sm['selfdriveStateSP'] + + if ss_sp.mads.available: + return bool(ss_sp.mads.active) + + # MADS not available, use stock state to engage + return bool(sm['selfdriveState'].active) + + @staticmethod + def state_control_ext(sm: messaging.SubMaster) -> custom.CarControlSP: + CC_SP = custom.CarControlSP.new_message() + + # MADS state + CC_SP.mads = sm['selfdriveStateSP'].mads + + return CC_SP + + @staticmethod + def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: + cc_sp_send = messaging.new_message('carControlSP') + cc_sp_send.valid = sm['carState'].canValid + cc_sp_send.carControlSP = CC_SP + + pm.send('carControlSP', cc_sp_send) + + def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: + CC_SP = self.state_control_ext(sm) + self.publish_ext(CC_SP, sm, pm) diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 979de0a80..67672649c 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -134,4 +134,8 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.WARNING: wrong_car_mode_alert, }, + EventNameSP.pedalPressedAlertOnly: { + ET.WARNING: NoEntryAlert("Pedal Pressed") + } + }