diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 816933f2f..56eefb0f0 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -76,7 +76,7 @@ class CarController: stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.starting can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel, - acc_control, stopping, starting, CS.out.cruiseState.standstill)) + acc_control, stopping, starting, CS.out.esp_hold_confirmation)) # **** HUD Controls ***************************************************** # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 2e43b6805..8669f28db 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -118,7 +118,8 @@ class CarState(CarStateBase): # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) ret.cruiseState.available = False ret.cruiseState.enabled = False - ret.cruiseState.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) # Update ACC setpoint. When the setpoint is zero or there's an error, the diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index e0b6c8260..04d19ac93 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -56,7 +56,7 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return acc_control_value(main_switch_on, acc_faulted, long_active) -def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, standstill): +def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold): commands = [] acc_06_values = { @@ -75,7 +75,7 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, if starting: acc_hold_type = 4 # hold release / startup - elif standstill: + elif esp_hold: acc_hold_type = 3 # hold standby elif stopping: acc_hold_type = 1 # hold request diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 30f3fcf62..0bcbf6abb 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -59,7 +59,7 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return hud_status -def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, standstill): +def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold): commands = [] values = { diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 476fa87fb..f72995d41 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -23,6 +23,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, starting_condition = (v_target_1sec > CP.vEgoStarting and accelerating and + not cruise_standstill and not brake_pressed) started_condition = v_ego > CP.vEgoStarting @@ -44,7 +45,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, long_control_state = LongCtrlState.pid elif long_control_state == LongCtrlState.starting: - if planned_stop: + if stopping_condition: long_control_state = LongCtrlState.stopping elif started_condition: long_control_state = LongCtrlState.pid