diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 8c58d3db04..365dec9723 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -6,7 +6,6 @@ from opendbc.can.packer import CANPacker class CarController(): def __init__(self, dbc_name, CP, VM): - self.braking = False self.apply_steer_last = 0 self.ccframe = 0 self.prev_frame = -1 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index edf48a779b..827cdd7f42 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -73,22 +73,13 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [] # events - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed = 2.) - - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed=2.) if ret.vEgo < self.CP.minSteerSpeed: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6a5992406a..9ebaad366f 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -61,23 +61,12 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - # enable request in prius is simple, as we activate when Toyota is active (rising edge) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled: events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() - return self.CS.out # pass in a car.CarControl diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 48b744a928..7ac1e58129 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -159,15 +159,10 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents - events = self.create_common_events(ret) + supercruise = self.CS.car_fingerprint in SUPERCRUISE_CARS + events = self.create_common_events(ret, pcm_enable=supercruise) - if self.CS.car_fingerprint in SUPERCRUISE_CARS: - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - - else: + if not supercruise: # TODO: why is this only not supercruise? ignore supercruise? if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) @@ -189,11 +184,6 @@ class CarInterface(CarInterfaceBase): ret.events = events - # update previous brake/gas pressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index de43c4cacf..705fc7c2ae 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -454,7 +454,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents # events - events = self.create_common_events(ret) + events = self.create_common_events(ret, pcm_enable=False) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: @@ -506,10 +506,6 @@ class CarInterface(CarInterfaceBase): ret.events = events - # update previous brake/gas pressed - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c990a14c65..c8246e124f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -109,11 +109,6 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # 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.: self.low_speed_alert = True @@ -124,10 +119,6 @@ class CarInterface(CarInterfaceBase): ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a25df91f9b..c85ad04e5e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -17,9 +17,6 @@ class CarInterfaceBase(): self.VM = VehicleModel(CP) self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False self.low_speed_alert = False self.CS = CarState(CP) @@ -81,7 +78,7 @@ class CarInterfaceBase(): def apply(self, c): raise NotImplementedError - def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): + def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1, pcm_enable=True): events = [] if cs_out.doorOpen: @@ -108,10 +105,17 @@ class CarInterfaceBase(): # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ - (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + # we engage when pcm is active (rising edge) + if pcm_enable: + if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled: + events.append(create_event('pcmEnable', [ET.ENABLE])) + elif not cs_out.cruiseState.enabled: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + return events class RadarInterfaceBase(): @@ -133,6 +137,7 @@ class CarStateBase: self.CP = CP self.car_fingerprint = CP.carFingerprint self.cruise_buttons = 0 + self.out = car.CarState.new_message() # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 676ef66a3f..8fc948c38c 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -75,22 +75,11 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - if self.CS.lkas_enabled: events.append(create_event('invalidLkasSetting', [ET.PERMANENT])) ret.events = events - # update previous brake/gas pressed - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 5cc3440997..7340b33330 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.car.subaru.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -66,18 +65,7 @@ class CarInterface(CarInterfaceBase): be.type = car.CarState.ButtonEvent.Type.accelCruise buttonEvents.append(be) - events = self.create_common_events(ret) - - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - - ret.events = events - - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled + ret.events = self.create_common_events(ret) self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6f21fec496..94f7f16565 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -29,25 +29,10 @@ def accel_hysteresis(accel, accel_steady, enabled): return accel, accel_steady -def process_hud_alert(hud_alert): - # initialize to no alert - steer = 0 - fcw = 0 - - if hud_alert == VisualAlert.fcw: - fcw = 1 - elif hud_alert == VisualAlert.steerRequired: - steer = 1 - - return steer, fcw - - class CarController(): def __init__(self, dbc_name, CP, VM): - self.braking = False self.last_steer = 0 self.accel_steady = 0. - self.car_fingerprint = CP.carFingerprint self.alert_active = False self.last_standstill = False self.standstill_req = False @@ -142,30 +127,28 @@ class CarController(): # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying - alert_out = process_hud_alert(hud_alert) - steer, fcw = alert_out + fcw_alert = hud_alert == VisualAlert.fcw + steer_alert = hud_alert == VisualAlert.steerRequired - if (any(alert_out) and not self.alert_active) or \ - (not any(alert_out) and self.alert_active): + send_ui = False + if ((fcw_alert or steer_alert) and not self.alert_active) or \ + (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active - else: - send_ui = False - - # disengage msg causes a bad fault sound so play a good sound instead - if pcm_cancel_cmd: + elif pcm_cancel_cmd: + # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus: - can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and Ecu.dsu in self.fake_ecus: - can_sends.append(create_fcw_command(self.packer, fcw)) + can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: - if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: + if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a6bea00f6a..cc3bea8dee 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -310,18 +310,8 @@ class CarInterface(CarInterfaceBase): # while in standstill, send a user alert events.append(create_event('manualRestart', [ET.WARNING])) - # enable request in prius is simple, as we activate when Toyota is active (rising edge) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 67a9ec3c19..cc4e978cb8 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -112,22 +112,11 @@ class CarInterface(CarInterfaceBase): if self.CS.steeringFault: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - # Engagement and longitudinal control using stock ACC. Make sure OP is - # disengaged if stock ACC is disengaged. - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # Attempt OP engagement only on rising edge of stock ACC engagement. - elif not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - ret.events = events ret.buttonEvents = buttonEvents ret.canMonoTimes = canMonoTimes # update previous car states - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.buttonStatesPrev = self.CS.buttonStates.copy()