mawei
/
sunnypilot
mirror of https://github.com/sunnypilot/sunnypilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

car: Interface cleanup (#396) 

* deprecated

* cruise main state

* v cruise non pcm state

* acc mads combo

* cancel cruise state

* sp common state

* sp event

* custom stock long

* started mads

* v cruise non pcm state old

* acc mads old

* started mads old
This commit is contained in:
Jason Wen 2024-08-13 11:57:27 -08:00 committed by GitHub
parent ddf69de7e0
commit 5b674d2231
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 127 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal/car.capnp
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;

21
selfdrive/car/chrysler/interface.py
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, self.CS.accEnabled,
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, self.CS.madsEnabled)
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, self.CS.madsEnabled)
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()

15
selfdrive/car/ford/interface.py
View File

@ -79,10 +79,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, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@ -90,15 +89,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, 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 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:
@ -106,7 +105,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,
@ -115,7 +114,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)

15
selfdrive/car/gm/interface.py
View File

@ -221,14 +221,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, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@ -236,15 +235,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, 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 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:
@ -252,7 +251,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,
@ -267,7 +266,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

19
selfdrive/car/honda/interface.py
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, self.CS.accEnabled)
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, self.CS.madsEnabled)
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()

21
selfdrive/car/hyundai/interface.py
View File

@ -230,10 +230,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, self.CS.accEnabled)
if ret.cruiseState.available:
if not self.CP.pcmCruiseSpeed:
@ -246,7 +245,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, self.CS.madsEnabled)
if not ret.cruiseState.available and self.CS.out.cruiseState.available:
self.CS.madsEnabled = False
@ -254,15 +253,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,
@ -276,7 +275,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.:
@ -289,9 +288,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()

103
selfdrive/car/interfaces.py
View File

@ -550,20 +550,20 @@ 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, mads_enabled):
if self.CS.params_list.acc_mads_combo:
if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled):
if not self.prev_acc_mads_combo and (cs_out.cruiseState.enabled or self.CS.accEnabled):
mads_enabled = True
self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled)
self.prev_acc_mads_combo = (cs_out.cruiseState.enabled or self.CS.accEnabled)
return mads_enabled
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, acc_enabled,
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
@ -578,9 +578,9 @@ class CarInterfaceBase(ABC):
return acc_enabled
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, mads_enabled):
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 mads_enabled
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 mads_enabled
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

19
selfdrive/car/mazda/interface.py
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, self.CS.accEnabled)
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, self.CS.madsEnabled)
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()

10
selfdrive/car/nissan/interface.py
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, 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):
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)

10
selfdrive/car/subaru/interface.py
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, self.CS.madsEnabled)
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()

10
selfdrive/car/toyota/interface.py
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, self.CS.madsEnabled)
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

22
selfdrive/car/volkswagen/interface.py
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, self.CS.accEnabled,
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, self.CS.madsEnabled)
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, self.CS.madsEnabled)
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()