diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 0cfbefdf8..5b3d64f97 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -66,10 +66,11 @@ class CarController(): 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 + secondary_accel = accel if lead_visible else 0 # try matching part of stock behavior, presumably drag/compression brake only if we can get away with it 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, + accel, secondary_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, acc_hold_request, acc_hold_release, diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 32cc88ca4..95ca059aa 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -85,7 +85,7 @@ 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_hold_request, acc_hold_release, +def create_mqb_acc_07_control(packer, bus, enabled, accel, secondary_accel, acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance, idx): values = { "ACC_Distance_to_Stop": stopping_distance, @@ -93,7 +93,7 @@ def create_mqb_acc_07_control(packer, bus, enabled, accel, acc_hold_request, acc "ACC_Freewheel_Type": 2 if enabled else 0, "ACC_Hold_Type": acc_hold_type, "ACC_Hold_Release": acc_hold_release, - "ACC_Accel_Secondary": accel+0.01 if enabled else 3.03, + "ACC_Accel_Secondary": secondary_accel+0.01 if enabled else 3.03, # FIXME: think there are rounding/scaling issues here "ACC_Accel_TSK": accel if enabled else 3.01, }