diff --git a/cereal/car.capnp b/cereal/car.capnp index 38be2d9441..2448ef3c9a 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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; diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ec1587e4e4..0c3e6f09e4 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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() diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 5461590679..9392132ce2 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -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) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 35e4654597..17ca61d8cc 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 1459b301a2..3d80470a23 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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() diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 750a76e555..47d414ea56 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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() diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index aca6398701..ae6cfeb55b 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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 diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 4c26a18a57..5e782f7344 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -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() diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 661ac668a5..92b03c716e 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -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) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 185ee8e54d..cba5a27461 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -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() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index dfa05f8fcd..b15c2d0b89 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0d57539a1b..aeccba278d 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -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()