diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5624c3dd5f..816933f2f0 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -7,6 +7,7 @@ from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState class CarController: @@ -25,7 +26,6 @@ class CarController: def update(self, CC, CS, ext_bus): actuators = CC.actuators hud_control = CC.hudControl - can_sends = [] # **** Steering Controls ************************************************ # @@ -71,9 +71,12 @@ class CarController: # **** Acceleration Controls ******************************************** # if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: - tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 - can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel)) + 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)) # **** HUD Controls ***************************************************** # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index facc740a15..cf4a252b65 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -215,6 +215,7 @@ class CarState(CarStateBase): ret.stockAeb = False # Update ACC radar status. + self.acc_type = 0 # TODO: this is ACC "basic" with nonzero min speed, support FtS (1) later ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) if self.CP.pcmCruise: diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3ed7a6244d..821eef44c7 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -38,6 +38,7 @@ class CarInterface(CarInterfaceBase): if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1 ret.networkLocation = NetworkLocation.gateway + ret.experimentalLongitudinalAvailable = True else: ret.networkLocation = NetworkLocation.fwdCamera @@ -49,13 +50,6 @@ class CarInterface(CarInterfaceBase): # Panda ALLOW_DEBUG firmware required. ret.dashcamOnly = True - if experimental_long and ret.networkLocation == NetworkLocation.gateway: - # Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should - # be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required. - ret.experimentalLongitudinalAvailable = True - ret.openpilotLongitudinalControl = True - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL - else: # Set global MQB parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] @@ -87,6 +81,13 @@ class CarInterface(CarInterfaceBase): # Global longitudinal tuning defaults, can be overridden per-vehicle + if experimental_long and candidate in PQ_CARS: + # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required. + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL + if ret.transmissionType == TransmissionType.manual: + ret.minEnableSpeed = 4.5 + ret.pcmCruise = not ret.openpilotLongitudinalControl ret.longitudinalActuatorDelayUpperBound = 0.5 # s ret.longitudinalTuning.kpV = [0.1] diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index e64bb2246e..30f3fcf62d 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -35,15 +35,15 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=Fa return packer.make_can_msg("GRA_Neu", bus, values) -def tsk_status_value(main_switch_on, acc_faulted, long_active): +def acc_control_value(main_switch_on, acc_faulted, long_active): if long_active: - tsk_status = 1 + acc_control = 1 elif main_switch_on: - tsk_status = 2 + acc_control = 2 else: - tsk_status = 0 + acc_control = 0 - return tsk_status + return acc_control def acc_hud_status_value(main_switch_on, acc_faulted, long_active): @@ -59,26 +59,32 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return hud_status -def create_acc_accel_control(packer, bus, adr_status, accel): +def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, standstill): + commands = [] + values = { - "ACS_Sta_ADR": adr_status, - "ACS_StSt_Info": adr_status != 1, - "ACS_Typ_ACC": 0, # TODO: this is ACC "basic", find a way to detect FtS support (1) - "ACS_Sollbeschl": accel if adr_status == 1 else 3.01, - "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27, - "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08, + "ACS_Sta_ADR": acc_control, + "ACS_StSt_Info": acc_control != 1, + "ACS_Typ_ACC": acc_type, + "ACS_Sollbeschl": accel if acc_control == 1 else 3.01, + "ACS_zul_Regelabw": 0.2 if acc_control == 1 else 1.27, + "ACS_max_AendGrad": 3.0 if acc_control == 1 else 5.08, } - return packer.make_can_msg("ACC_System", bus, values) + commands.append(packer.make_can_msg("ACC_System", bus, values)) + + return commands -def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): values = { - "ACA_StaACC": acc_status, + "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": 8 if lead_visible else 0, + # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke + # display/display-prio handling probably needed to stop confusing the instrument cluster + # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint } - # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke return packer.make_can_msg("ACC_GRA_Anziege", bus, values) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 7c55736362..6425cd60be 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -20,7 +20,6 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz - ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz ACCEL_MAX = 2.0 # 2.0 m/s max acceleration ACCEL_MIN = -3.5 # 3.5 m/s max deceleration @@ -37,6 +36,7 @@ class CarControllerParams: if CP.carFingerprint in PQ_CARS: self.LDW_STEP = 5 # LDW_1 message frequency 20Hz + self.ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00)) self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) @@ -65,6 +65,7 @@ class CarControllerParams: else: self.LDW_STEP = 10 # LDW_02 message frequency 10Hz + self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))