From c0792fe57a71bbee6dceb3fc84678fdeb854a1b9 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 24 Nov 2021 07:43:39 -0500 Subject: [PATCH] the code is darkest before the refactor --- selfdrive/car/volkswagen/carcontroller.py | 39 ++++++++++++----------- selfdrive/car/volkswagen/interface.py | 10 +++--- selfdrive/car/volkswagen/volkswagencan.py | 22 ++++++------- 3 files changed, 34 insertions(+), 37 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 8994ac2dd2..760db71f03 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -41,41 +41,44 @@ class CarController(): else: acc_status = CS.tsk_status - accel = actuators.accel if enabled else 0 - accel = clip(accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0 + accel = clip(actuators.accel, P.ACCEL_MIN, P.ACCEL_MAX) if enabled else 0 # FIXME: this needs to become a proper state machine - acc_hold_request, acc_hold_release = False, False - if actuators.longControlState == LongCtrlState.stopping: + acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance = False, False, 0, 0 + if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < 0.2: self.acc_stopping = True - acc_hold_request = not CS.esp_hold_confirmation # Send hold request for ABS/ESP brake-hold until acked + if CS.esp_hold_confirmation: + acc_hold_type = 1 # hold + stopping_distance = 3.5 + else: + acc_hold_type = 3 # hold_standby + stopping_distance = 0.5 elif enabled: if self.acc_stopping: self.acc_starting = True self.acc_stopping = False - self.acc_starting &= CS.out.vEgo < CS.CP.vEgoStarting - acc_hold_release = CS.esp_hold_confirmation # Send release request for ABS/ESP brake-hold until acked + self.acc_starting &= CS.out.vEgo < 1.5 + acc_hold_release = self.acc_starting + stopping_distance = 20.46 + if CS.esp_hold_confirmation: + acc_hold_type = 4 # startup + else: + acc_hold_type = 0 # no_request else: self.acc_stopping, self.acc_starting = False, False - # Hopefully, have PI yank the lag out and let the drivetrain coordinator make it smooth again - cb_pos = 0.0 if lead_visible or CS.out.vEgo < 2.0 else 0.2 # react faster to lead cars, also don't get hung up at DSG clutch release/kiss points when creeping to stop - cb_neg = 0.0 if accel < 0 else 0.2 # IDK why, but stock likes to zero this out when accel is negative + acc_hold_request = self.acc_stopping or (self.acc_starting and CS.esp_hold_confirmation) - if acc_hold_request: - weird_value = 0x88 - elif self.acc_stopping: - weird_value = 0x95 - else: - weird_value = 0x7F + cb_pos = 0.0 if lead_visible or CS.out.vEgo < 2.0 else 0.1 # react faster to lead cars, also don't get hung up at DSG clutch release/kiss points when creeping to stop + cb_neg = 0.0 if accel < 0 else 0.2 # IDK why, but stock likes to zero this out when accel is negative 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, self.acc_stopping, self.acc_starting, cb_pos, cb_neg, idx)) can_sends.append(volkswagencan.create_mqb_acc_07_control(self.packer_pt, CANBUS.pt, enabled, - accel, self.acc_stopping, self.acc_starting, - acc_hold_request, acc_hold_release, weird_value, idx)) + accel, acc_hold_request, acc_hold_release, + acc_hold_type, stopping_distance, idx)) if frame % P.ACC_HUD_STEP == 0: idx = (frame / P.ACC_HUD_STEP) % 16 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index e88587337c..3ef6376ae1 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -66,15 +66,13 @@ class CarInterface(CarInterfaceBase): # Global longitudinal tuning defaults, can be overridden per-vehicle ret.pcmCruise = not ret.openpilotLongitudinalControl - ret.longitudinalActuatorDelayLowerBound = 0.5 # s ret.longitudinalActuatorDelayUpperBound = 1.0 # s ret.stoppingControl = True - ret.vEgoStopping = 0.2 - ret.vEgoStarting = 0.2 - ret.stopAccel = 0.0 + ret.vEgoStopping = 0.4 + ret.vEgoStarting = 0.5 ret.startAccel = 0.0 - ret.startingAccelRate = 6.0 - ret.longitudinalTuning.kpV = [0.1] + ret.stopAccel = -1.0 + ret.longitudinalTuning.kpV = [0.0] ret.longitudinalTuning.kiV = [0.0] # Per-chassis tuning values, override tuning defaults here if desired diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index a52cb3479f..56c6145247 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -65,20 +65,16 @@ def create_mqb_acc_06_control(packer, bus, enabled, acc_status, accel, acc_stopp 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): +def create_mqb_acc_07_control(packer, bus, enabled, accel, acc_hold_request, acc_hold_release, + acc_hold_type, stopping_distance, 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, - "ACC_Sollbeschleunigung_01": accel if enabled else 3.01, - "ACC_Sollbeschleunigung_03": accel if enabled else 0, + "ACC_Distance_to_Stop": stopping_distance, + "ACC_Hold_Request": acc_hold_request, + "ACC_Freewheel_Type": 2, + "ACC_Hold_Type": acc_hold_type, + "ACC_Hold_Release": acc_hold_release, + "ACC_Accel_Secondary": accel if enabled else 0, + "ACC_Accel_Primary": accel if enabled else 3.01, } return packer.make_can_msg("ACC_07", bus, values, idx)