diff --git a/panda b/panda index 571512a392..b1205ad3bf 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 571512a392396271e54469c562b2ba08cbb44bfc +Subproject commit b1205ad3bffbb9f75e7747920e2b91f249620fb2 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 97f00008bd..e751d1863d 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,10 +1,13 @@ +from common.numpy_fast import clip from cereal import car +from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState class CarController(): def __init__(self, dbc_name, CP, VM): @@ -20,12 +23,49 @@ class CarController(): self.graMsgBusCounterPrev = 0 self.steer_rate_limited = False + self.openpilot_stopping = False - def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, + right_lane_depart, lead_visible, set_speed): """ Controls thread """ can_sends = [] + # **** Acceleration and Braking Controls ******************************** # + + if CS.CP.openpilotLongitudinalControl: + if CS.tsk_status in [2, 3, 4, 5]: + acc_status = 3 if enabled else 2 + else: + acc_status = CS.tsk_status + + accel = clip(actuators.accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0 + jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) + + acc_stopping, acc_starting, acc_hold_request, acc_hold_release = False, False, False, False + if actuators.longControlState == LongCtrlState.stopping: + accel = -1.0 + acc_stopping = True + acc_hold_request = not CS.esp_hold_confirmation + elif enabled: + acc_starting = CS.out.vEgo < 0.2 + acc_hold_release = CS.esp_hold_confirmation + + if acc_hold_request: + weird_value = 0x88 + elif acc_stopping: + weird_value = 0x95 + else: + weird_value = 0x7F + + if frame % P.ACC_CONTROL_STEP == 0: + idx = (frame / P.ACC_CONTROL_STEP) % 16 + can_sends.append(volkswagencan.create_mqb_acc_06_control(self.packer_pt, CANBUS.pt, enabled, acc_status, + accel, jerk, acc_stopping, acc_starting, idx)) + can_sends.append(volkswagencan.create_mqb_acc_07_control(self.packer_pt, CANBUS.pt, enabled, + accel, acc_stopping, acc_starting, + acc_hold_request, acc_hold_release, weird_value, idx)) + # **** Steering Controls ************************************************ # if frame % P.HCA_STEP == 0: @@ -82,31 +122,39 @@ class CarController(): right_lane_visible, CS.ldw_stock_values, left_lane_depart, right_lane_depart)) + if CS.CP.openpilotLongitudinalControl: + if frame % P.ACC_HUD_STEP == 0: + idx = (frame / P.ACC_HUD_STEP) % 16 + can_sends.append(volkswagencan.create_mqb_acc_hud_control(self.packer_pt, CANBUS.pt, CS.tsk_status, + set_speed * CV.MS_TO_KPH, idx)) + # **** ACC Button Controls ********************************************** # # FIXME: this entire section is in desperate need of refactoring - if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.out.cruiseState.enabled: - # Cancel ACC if it's engaged with OP disengaged. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.out.standstill: - # Blip the Resume button if we're engaged at standstill. - # FIXME: This is a naive implementation, improve with visiond or radar input. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["resumeCruise"] = True - - if CS.graMsgBusCounter != self.graMsgBusCounterPrev: - self.graMsgBusCounterPrev = CS.graMsgBusCounter - if self.graButtonStatesToSend is not None: - if self.graMsgSentCount == 0: - self.graMsgStartFramePrev = frame - idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) - self.graMsgSentCount += 1 - if self.graMsgSentCount >= P.GRA_VBP_COUNT: - self.graButtonStatesToSend = None - self.graMsgSentCount = 0 + if not CS.CP.openpilotLongitudinalControl: + if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: + if not enabled and CS.out.cruiseState.enabled: + # Cancel ACC if it's engaged with OP disengaged. + self.graButtonStatesToSend = BUTTON_STATES.copy() + self.graButtonStatesToSend["cancel"] = True + elif enabled and CS.out.standstill and CS.esp_hold_confirmation: + # Blip the Resume button if we're engaged at standstill. + # FIXME: This is a naive implementation, improve with visiond or radar input. + # A subset of MQBs like to "creep" too aggressively with this implementation. + self.graButtonStatesToSend = BUTTON_STATES.copy() + self.graButtonStatesToSend["resumeCruise"] = True + + if CS.graMsgBusCounter != self.graMsgBusCounterPrev: + self.graMsgBusCounterPrev = CS.graMsgBusCounter + if self.graButtonStatesToSend is not None: + if self.graMsgSentCount == 0: + self.graMsgStartFramePrev = frame + idx = (CS.graMsgBusCounter + 1) % 16 + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) + self.graMsgSentCount += 1 + if self.graMsgSentCount >= P.GRA_VBP_COUNT: + self.graButtonStatesToSend = None + self.graMsgSentCount = 0 return can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index f02129a61c..913cd3abbc 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -27,7 +27,7 @@ class CarState(CarStateBase): ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.standstill = ret.vEgoRaw < 0.1 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. @@ -47,6 +47,7 @@ class CarState(CarStateBase): ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"] # Update gear and/or clutch position data. if trans_type == TransmissionType.automatic: @@ -94,13 +95,13 @@ class CarState(CarStateBase): ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) # Update ACC radar status. - accStatus = pt_cp.vl["TSK_06"]["TSK_Status"] - if accStatus == 2: + self.tsk_status = pt_cp.vl["TSK_06"]["TSK_Status"] + if self.tsk_status == 2: # ACC okay and enabled, but not currently engaged ret.cruiseState.available = True ret.cruiseState.enabled = False - elif accStatus in [3, 4, 5]: - # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) + elif self.tsk_status in [3, 4, 5]: + # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) ret.cruiseState.available = True ret.cruiseState.enabled = True else: @@ -108,11 +109,12 @@ class CarState(CarStateBase): ret.cruiseState.available = False ret.cruiseState.enabled = False - # Update ACC setpoint. When the setpoint is zero or there's an error, the - # radar sends a set-speed of ~90.69 m/s / 203mph. - ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS - if ret.cruiseState.speed > 90: - ret.cruiseState.speed = 0 + if not self.CP.openpilotLongitudinalControl: + # Update ACC setpoint. When the setpoint is zero or there's an error, the + # radar sends a set-speed of ~90.69 m/s / 203mph. + ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS + if ret.cruiseState.speed > 90: + ret.cruiseState.speed = 0 # Update control button states for turn signals and ACC controls. self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"]) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index d5b346f410..08b570f2f3 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,8 +1,11 @@ from cereal import car +from common.params import Params +from panda import Panda from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -31,6 +34,14 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 + # Disable the radar and let openpilot control longitudinal + # WARNING: THIS DISABLES FACTORY FCW/AEB! + if Params().get_bool("DisableRadar"): + ret.pcmCruise = False + ret.openpilotLongitudinalControl = True + ret.directAccelControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONGITUDINAL + if 0xAD in fingerprint[0]: # Getriebe_11 ret.transmissionType = TransmissionType.automatic elif 0x187 in fingerprint[0]: # EV_Gearshift @@ -43,7 +54,7 @@ class CarInterface(CarInterfaceBase): else: ret.networkLocation = NetworkLocation.fwdCamera - # Global tuning defaults, can be overridden per-vehicle + # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.05 ret.steerRateCost = 1.0 @@ -56,6 +67,17 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV = [0.6] ret.lateralTuning.pid.kiV = [0.2] + # Global longitudinal tuning defaults, can be overridden per-vehicle + + ret.longitudinalActuatorDelayUpperBound = 1.0 # s + ret.minSpeedCan = 0.0 + ret.stoppingControl = True + ret.vEgoStopping = 1.0 + ret.stopAccel = 0.0 + ret.startAccel = 0.0 + ret.longitudinalTuning.kpV = [0.1] + ret.longitudinalTuning.kiV = [0.0] + # Per-chassis tuning values, override tuning defaults here if desired if candidate == CAR.ARTEON_MK1: @@ -181,11 +203,14 @@ class CarInterface(CarInterfaceBase): be.pressed = self.CS.buttonStates[button] buttonEvents.append(be) - events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic]) + events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], + pcm_enable=not self.CS.CP.openpilotLongitudinalControl) # Vehicle health and operation safety checks if self.CS.parkingBrakeSet: events.add(EventName.parkBrake) + if self.CS.tsk_status in [6, 7]: + events.add(EventName.accFaulted) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): @@ -195,6 +220,15 @@ class CarInterface(CarInterfaceBase): if self.low_speed_alert: events.add(EventName.belowSteerSpeed) + if self.CS.CP.openpilotLongitudinalControl: + for b in buttonEvents: + # do enable on falling edge of both accel and decel buttons + if b.type in [ButtonType.setCruise, ButtonType.resumeCruise] and not b.pressed: + events.add(EventName.buttonEnable) + # do disable on rising edge of cancel + if b.type == "cancel" and b.pressed: + events.add(EventName.buttonCancel) + ret.events = events.to_msg() ret.buttonEvents = buttonEvents @@ -211,6 +245,8 @@ class CarInterface(CarInterfaceBase): c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, - c.hudControl.rightLaneDepart) + c.hudControl.rightLaneDepart, + c.hudControl.leadVisible, + c.hudControl.setSpeed) self.frame += 1 return can_sends diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index ed0b1ac76d..52fcd9de6d 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -15,6 +15,8 @@ class CarControllerParams: HCA_STEP = 2 # HCA_01 message frequency 50Hz LDW_STEP = 10 # LDW_02 message frequency 10Hz GRA_ACC_STEP = 3 # GRA_ACC_01 message frequency 33Hz + ACC_CONTROL_STEP = 2 # ACC_06 message frequency 50Hz + ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) @@ -29,6 +31,9 @@ class CarControllerParams: STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily STEER_DRIVER_FACTOR = 1 # from dbc + ACCEL_MAX = 2.0 # 2.0 m/s max acceleration + ACCEL_MIN = -3.5 # 3.5 m/s max deceleration + class CANBUS: pt = 0 cam = 2 diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 10e0054c79..544d21c8b2 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -47,3 +47,48 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): "GRA_ButtonTypeInfo": CS.graButtonTypeInfo } return packer.make_can_msg("GRA_ACC_01", bus, values, idx) + +def create_mqb_acc_06_control(packer, bus, enabled, acc_status, accel, jerk, acc_stopping, acc_starting, + idx): + values = { + "ACC_Typ": 2, # FIXME: locked to stop and go, need to tweak for cars that only support follow-to-stop + "ACC_Status_ACC": acc_status, + "ACC_StartStopp_Info": enabled, + "ACC_Sollbeschleunigung_02": accel if enabled else 3.01, + "ACC_zul_Regelabw_unten": 0.2 if enabled else 0, # FIXME: need comfort regulation logic here + "ACC_zul_Regelabw_oben": 0.1 if enabled else 0, # FIXME: need comfort regulation logic here + "ACC_neg_Sollbeschl_Grad_02": jerk if enabled else 0, + "ACC_pos_Sollbeschl_Grad_02": jerk if enabled else 0, + "ACC_Anfahren": acc_starting, + "ACC_Anhalten": acc_stopping, + } + + return packer.make_can_msg("ACC_06", bus, values, idx) + +def create_mqb_acc_07_control(packer, bus, enabled, accel, acc_stopping, acc_starting, acc_hold_request, + acc_hold_release, weird_value, idx): + values = { + "XXX_Maybe_Engine_Start_Request": 2, # TODO + "XXX_Always_1": 1, + "XXX_Maybe_Engine_Stop_Release": not acc_hold_request, # TODO this isn't S/S, AHR but also slight delay past AHR + "XXX_Unknown": weird_value, # FIXME: Should try to figure out what's really going on here + "ACC_Engaged": enabled, + "ACC_Anhalten": acc_stopping, + "ACC_Anhaltevorgang": acc_hold_request, + "ACC_Anfahrvorgang": acc_hold_release, + "ACC_Anfahren": acc_starting, + "XXX_Lead_Car_Relative_Speed": 0x65, + "ACC_Sollbeschleunigung_01": accel if enabled else 3.01, + } + + return packer.make_can_msg("ACC_07", bus, values, idx) + +def create_mqb_acc_hud_control(packer, bus, acc_status, set_speed, idx): + values = { + "ACC_Status_Anzeige": acc_status, + "ACC_Wunschgeschw": 327.36 if set_speed == 0 else set_speed, + "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Display_Prio": 3 + } + + return packer.make_can_msg("ACC_02", bus, values, idx)