Merge branch 'master-new' of https://github.com/sunnypilot/sunnypilot into master-new

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;

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()
 
 

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__":

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

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)

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")
+  }
+
 }