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

Merge branch 'car-interface-cleanup' into master-dev-c3

Browse Source 
# Conflicts:
#	selfdrive/car/hyundai/interface.py
This commit is contained in:
Jason Wen
2024-08-11 17:54:55 -04:00
parent 9183330354 b3543efcb3
commit 3a1eba95bf
12 changed files with 134 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/car.capnp
+1 -1
View File
@@ -251,7 +251,7 @@ struct CarState {
struct CustomStockLong {
cruiseButton @0 :Int16;
finalSpeedKph @1 :Float32;
vCruiseKphPrev @2 :Float32;
vCruiseKphPrevDEPRECATED @2 :Float32;
targetSpeed @3 :Float32;
vSetDis @4 :Float32;
speedDiff @5 :Float32;
selfdrive/car/chrysler/interface.py
+9 -12
View File
@@ -98,10 +98,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise,
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) if not self.CP.pcmCruiseSpeed else
(ButtonType.accelCruise, ButtonType.decelCruise),
resume_button=(ButtonType.resumeCruise,) if not self.CP.pcmCruiseSpeed else
@@ -114,16 +113,16 @@ class CarInterface(CarInterfaceBase):
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.lkas_disabled = not self.CS.lkas_disabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
self.CS.madsEnabled = self.get_sp_started_mads(ret)
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -136,7 +135,7 @@ class CarInterface(CarInterfaceBase):
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, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -146,7 +145,7 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret, c, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
# Low speed steer alert hysteresis logic
if self.CP.carFingerprint in RAM_DT:
@@ -162,9 +161,7 @@ class CarInterface(CarInterfaceBase):
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
selfdrive/car/ford/interface.py
+7 -8
View File
@@ -89,10 +89,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
@@ -100,15 +99,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
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 self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -116,7 +115,7 @@ class CarInterface(CarInterfaceBase):
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, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -125,7 +124,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.manumatic], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
if not self.CS.vehicle_sensors_valid:
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
selfdrive/car/gm/interface.py
+7 -8
View File
@@ -222,14 +222,13 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in self.CS.button_events):
self.CS.accEnabled = True
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
@@ -237,15 +236,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -253,7 +252,7 @@ class CarInterface(CarInterfaceBase):
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, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -268,7 +267,7 @@ class CarInterface(CarInterfaceBase):
# 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,
events, ret = self.create_sp_events(ret, events, enable_pressed=self.CS.accEnabled,
enable_buttons=(ButtonType.decelCruise,))
# Enabling at a standstill with brake is allowed
selfdrive/car/honda/interface.py
+8 -11
View File
@@ -267,10 +267,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
@@ -278,7 +277,7 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
@@ -286,9 +285,9 @@ class CarInterface(CarInterfaceBase):
if not self.CP.pcmCruise or min_enable_speed_pcm or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads or min_enable_speed_pcm \
else False if self.CP.pcmCruise \
else self.CS.accEnabled
@@ -301,7 +300,7 @@ class CarInterface(CarInterfaceBase):
elif not ret.cruiseState.enabled:
self.CS.accEnabled = False
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3))
ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_setting == 3))
ret.buttonEvents = [
*self.CS.button_events,
@@ -313,7 +312,7 @@ class CarInterface(CarInterfaceBase):
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed and not self.CS.madsEnabled:
events.add(EventName.belowEngageSpeed)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
#if self.CP.pcmCruise:
# # we engage when pcm is active (rising edge)
@@ -330,9 +329,7 @@ class CarInterface(CarInterfaceBase):
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
selfdrive/car/hyundai/interface.py
+9 -12
View File
@@ -238,10 +238,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.main_buttons[-1], self.CS.prev_main_buttons, {1: ButtonType.altButton3}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise)
if ret.cruiseState.available:
if not self.CP.pcmCruiseSpeed:
@@ -254,7 +253,7 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
if not ret.cruiseState.available and self.CS.out.cruiseState.available:
self.CS.madsEnabled = False
@@ -262,15 +261,15 @@ class CarInterface(CarInterfaceBase):
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
if not self.CP.pcmCruise:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if not self.CP.pcmCruiseSpeed:
if not ret.cruiseState.enabled:
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3))
ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_buttons[-1] == 3))
ret.buttonEvents = [
*self.CS.button_events,
@@ -284,7 +283,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, 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)
events, ret = self.create_sp_events(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.:
@@ -297,9 +296,7 @@ class CarInterface(CarInterfaceBase):
if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache:
events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
selfdrive/car/interfaces.py
+61 -56
View File
@@ -550,37 +550,37 @@ class CarInterfaceBase(ABC):
def sp_v_cruise_initialized(v_cruise):
return v_cruise != V_CRUISE_UNSET
def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled):
def get_acc_mads(self, cs_out):
if self.CS.params_list.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)
if not self.prev_acc_mads_combo and (cs_out.cruiseState.enabled or self.CS.accEnabled):
self.CS.madsEnabled = True
self.prev_acc_mads_combo = (cs_out.cruiseState.enabled or self.CS.accEnabled)
return mads_enabled
return self.CS.madsEnabled
def get_sp_v_cruise_non_pcm_state(self, cs_out, acc_enabled, button_events, vCruise,
def get_sp_v_cruise_non_pcm_state(self, cs_out, vCruise,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise),
resume_button=(ButtonType.accelCruise, ButtonType.resumeCruise)):
if cs_out.cruiseState.available:
for b in button_events:
for b in self.CS.button_events:
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
if b.type in enable_buttons and not b.pressed:
acc_enabled = True
self.CS.accEnabled = True
if not self.CP.pcmCruise:
if b.type in resume_button and not self.sp_v_cruise_initialized(vCruise):
acc_enabled = False
self.CS.accEnabled = False
if not self.CP.pcmCruiseSpeed:
if b.type == ButtonType.accelCruise and not cs_out.cruiseState.enabled:
acc_enabled = False
self.CS.accEnabled = False
else:
acc_enabled = False
self.CS.accEnabled = False
return acc_enabled
return self.CS.accEnabled
def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False):
mads_enabled = False if not self.enable_mads or self.disengage_on_accelerator else mads_enabled
return mads_enabled, acc_enabled
def get_sp_cancel_cruise_state(self):
self.CS.madsEnabled = False if not self.enable_mads or self.disengage_on_accelerator else self.CS.madsEnabled
self.CS.accEnabled = False
def get_sp_pedal_disengage(self, cs_out):
accel_pedal = cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator
@@ -588,24 +588,22 @@ class CarInterfaceBase(ABC):
regen = cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)
return accel_pedal or brake or regen
def get_sp_cruise_main_state(self, cs_out, CS):
if not CS.control_initialized:
mads_enabled = False
def get_sp_cruise_main_state(self, cs_out):
if not self.CS.control_initialized:
return False
elif not self.CS.params_list.mads_main_toggle:
mads_enabled = False
return False
else:
mads_enabled = cs_out.cruiseState.available
return cs_out.cruiseState.available
return mads_enabled
def get_sp_started_mads(self, cs_out, CS):
if not cs_out.cruiseState.available and CS.out.cruiseState.available:
def get_sp_started_mads(self, cs_out):
if not cs_out.cruiseState.available and self.CS.out.cruiseState.available:
self.madsEnabledInit = False
self.madsEnabledInitPrev = False
return False
if not self.CS.params_list.mads_main_toggle or self.prev_acc_mads_combo:
return CS.madsEnabled
if not self.madsEnabledInit and CS.madsEnabled:
return self.CS.madsEnabled
if not self.madsEnabledInit and self.CS.madsEnabled:
self.madsEnabledInit = True
self.last_mads_init = time.monotonic()
if cs_out.gearShifter not in FORWARD_GEARS:
@@ -616,19 +614,19 @@ class CarInterfaceBase(ABC):
self.madsEnabledInitPrev = True
return cs_out.cruiseState.available
else:
return CS.madsEnabled
return self.CS.madsEnabled
def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False):
cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
def get_sp_common_state(self, cs_out, gear_allowed=True, gap_button=False):
cs_out.cruiseState.enabled = self.CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
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
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
self.CS.madsEnabled = True
elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
self.CS.madsEnabled = False
if self.CP.openpilotLongitudinalControl:
self.toggle_exp_mode(gap_button)
self.toggle_exp_mode(gap_button) # TODO-SP: use buttonEvents to handle this, then remove gap_button
lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric)
@@ -640,22 +638,22 @@ class CarInterfaceBase(ABC):
cs_out.latActive = gear_allowed
if not CS.control_initialized:
CS.control_initialized = True
if not self.CS.control_initialized:
self.CS.control_initialized = True
# 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)) or \
(cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)):
if CS.madsEnabled:
CS.disengageByBrake = True
if self.CS.madsEnabled:
self.CS.disengageByBrake = True
cs_out.madsEnabled = CS.madsEnabled
cs_out.accEnabled = CS.accEnabled
cs_out.disengageByBrake = CS.disengageByBrake
cs_out.madsEnabled = self.CS.madsEnabled
cs_out.accEnabled = self.CS.accEnabled
cs_out.disengageByBrake = self.CS.disengageByBrake
cs_out.brakeLightsDEPRECATED |= cs_out.brakePressed or cs_out.brakeHoldActive or cs_out.parkingBrake or cs_out.regenBraking
return cs_out, CS
return cs_out
# TODO: SP: use upstream's buttonEvents counter checks from controlsd
def toggle_exp_mode(self, gap_pressed):
@@ -670,7 +668,7 @@ class CarInterfaceBase(ABC):
self.gap_button_counter = 0
self.experimental_mode_hold = False
def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
def create_sp_events(self, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
enable_from_brake=False, enable_pressed_long=False,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
@@ -678,7 +676,7 @@ class CarInterfaceBase(ABC):
if cs_out.disengageByBrake and cs_out.madsEnabled:
enable_pressed = True
enable_from_brake = True
CS.disengageByBrake = False
self.CS.disengageByBrake = False
cs_out.disengageByBrake = False
for b in cs_out.buttonEvents:
@@ -707,11 +705,11 @@ class CarInterfaceBase(ABC):
if self.CP.pcmCruise:
# do disable on button down
if main_enabled:
if any(CS.main_buttons) and not cs_out.cruiseState.enabled:
if any(self.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:
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
enable_pressed = True
enable_pressed_long = True
elif not cs_out.cruiseState.enabled:
@@ -731,6 +729,16 @@ class CarInterfaceBase(ABC):
return events, cs_out
def update_custom_stock_long(self):
customStockLong = car.CarState.CustomStockLong.new_message()
customStockLong.cruiseButton = 0 if self.CC.cruise_button is None else int(self.CC.cruise_button)
customStockLong.finalSpeedKph = float(self.CC.final_speed_kph)
customStockLong.targetSpeed = float(self.CC.target_speed)
customStockLong.vSetDis = float(self.CC.v_set_dis)
customStockLong.speedDiff = float(self.CC.speed_diff)
customStockLong.buttonType = int(self.CC.button_type)
return customStockLong
class RadarInterfaceBase(ABC):
def __init__(self, CP):
self.CP = CP
@@ -834,16 +842,6 @@ class CarStateBase(ABC):
return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
def update_custom_stock_long(self, cruise_button, final_speed_kph, target_speed, v_set_dis, speed_diff, button_type):
customStockLong = car.CarState.CustomStockLong.new_message()
customStockLong.cruiseButton = 0 if cruise_button is None else cruise_button
customStockLong.finalSpeedKph = final_speed_kph
customStockLong.targetSpeed = target_speed
customStockLong.vSetDis = v_set_dis
customStockLong.speedDiff = speed_diff
customStockLong.buttonType = button_type
return customStockLong
@staticmethod
def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter:
if gear is None:
@@ -876,6 +874,13 @@ class CarControllerBase(ABC):
self.CP = CP
self.frame = 0
self.cruise_button = 0
self.final_speed_kph = 0.0
self.target_speed = 0.0
self.v_set_dis = 0.0
self.speed_diff = 0.0
self.button_type = 0
@abstractmethod
def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]:
pass
selfdrive/car/mazda/interface.py
+8 -11
View File
@@ -45,10 +45,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise)
if ret.cruiseState.available:
if self.enable_mads:
@@ -56,15 +55,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -72,7 +71,7 @@ class CarInterface(CarInterfaceBase):
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, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -83,16 +82,14 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
#if self.CS.lkas_disabled:
# events.add(EventName.lkasDisabled)
if self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
selfdrive/car/nissan/interface.py
+5 -5
View File
@@ -35,22 +35,22 @@ class CarInterface(CarInterfaceBase):
self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
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)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
selfdrive/car/subaru/interface.py
+5 -5
View File
@@ -117,7 +117,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if ret.cruiseState.available:
if self.enable_mads:
@@ -128,7 +128,7 @@ class CarInterface(CarInterfaceBase):
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)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
@@ -137,10 +137,10 @@ class CarInterface(CarInterfaceBase):
if not self.enable_mads:
self.CS.madsEnabled = False
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS)
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -150,7 +150,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
ret.events = events.to_msg()
selfdrive/car/toyota/interface.py
+5 -5
View File
@@ -214,7 +214,7 @@ class CarInterface(CarInterfaceBase):
self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
distance_button = self.CS.distance_button
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if ret.cruiseState.available:
if self.enable_mads:
@@ -229,16 +229,16 @@ class CarInterface(CarInterfaceBase):
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.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if not self.CP.pcmCruise:
ret.cruiseState.enabled = self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
ret.buttonEvents = [
*self.CS.button_events,
@@ -250,7 +250,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
# Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
# the more accurate angle sensor signal is initialized
selfdrive/car/volkswagen/interface.py
+9 -13
View File
@@ -114,26 +114,25 @@ 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 = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise,
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
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)
self.CS.madsEnabled = self.get_acc_mads(ret)
else:
self.CS.madsEnabled = False
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
self.CS.madsEnabled = self.get_sp_started_mads(ret)
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -141,8 +140,7 @@ class CarInterface(CarInterfaceBase):
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,
gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
ret = self.get_sp_common_state(ret, gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
ret.buttonEvents = [
*self.CS.button_events,
@@ -153,7 +151,7 @@ class CarInterface(CarInterfaceBase):
pcm_enable=False,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
events, ret = self.create_sp_events(self.CS, ret, events,
events, ret = self.create_sp_events(ret, events,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
# Low speed steer alert hysteresis logic
@@ -173,9 +171,7 @@ class CarInterface(CarInterfaceBase):
if self.CC.eps_timer_soft_disable_alert:
events.add(EventName.steerTimeLimit)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()