From f35c28a988db58052f58fea858640431eb8c71ce Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 7 Sep 2022 17:03:03 -0400 Subject: [PATCH] refactor all the things --- panda | 2 +- selfdrive/car/__init__.py | 2 +- selfdrive/car/volkswagen/carcontroller.py | 63 ++--------------- selfdrive/car/volkswagen/interface.py | 1 + selfdrive/car/volkswagen/mqbcan.py | 84 +++++++++++++---------- selfdrive/car/volkswagen/pqcan.py | 18 ++--- 6 files changed, 67 insertions(+), 103 deletions(-) diff --git a/panda b/panda index 0e8854b48..b0472024d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0e8854b48ff81cc61e30e9135091846ab5d9b887 +Subproject commit b0472024d324984d0453cc5d0ae506d58980bf91 diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index f2d198338..409939e67 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -29,7 +29,7 @@ def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilde for b in buttonEvents: # do enable on both accel and decel buttons if not pcm_cruise: - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: + if b.type in (ButtonType.setCruise, ButtonType.resumeCruise, ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: events.append(EventName.buttonEnable) # do disable on button down if b.type == ButtonType.cancel and b.pressed: diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index d12cc9ef0..e1a33a88a 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -15,6 +15,7 @@ class CarController: self.CP = CP self.CCP = CarControllerParams(CP) self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan + self.pass_through_enabled = CP.carFingerprint not in PQ_CARS self.packer_pt = CANPacker(dbc_name) self.apply_steer_last = 0 @@ -27,61 +28,8 @@ class CarController: def update(self, CC, CS, ext_bus): actuators = CC.actuators hud_control = CC.hudControl - can_sends = [] - # **** Acceleration and Braking Controls ******************************** # - - if CS.CP.openpilotLongitudinalControl: - if self.frame % self.CCP.ACC_CONTROL_STEP == 0: - if CS.tsk_status in [2, 3, 4, 5]: - acc_status = 3 if CC.longActive else 2 - else: - acc_status = CS.tsk_status - - accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 - - # FIXME: this needs to become a proper state machine - acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance = False, False, 0, 20.46 - if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < 0.2: - self.acc_stopping = True - acc_hold_request = True - if CS.out.cruiseState.standstill: - acc_hold_type = 1 # hold - stopping_distance = 3.5 - else: - acc_hold_type = 3 # hold_standby - stopping_distance = 0.5 - elif CC.longActive: - if self.acc_stopping: - self.acc_starting = True - self.acc_stopping = False - self.acc_starting &= CS.out.vEgo < 1.5 - acc_hold_release = self.acc_starting - acc_hold_type = 4 if self.acc_starting and CS.out.vEgo < 0.1 else 0 # startup - else: - self.acc_stopping, self.acc_starting = False, False - - cb_pos = 0.0 if hud_control.leadVisible 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 - - can_sends.append(self.CCS.create_acc_06_control(self.packer_pt, CANBUS.pt, CC.longActive, acc_status, - accel, self.acc_stopping, self.acc_starting, - cb_pos, cb_neg, CS.acc_type)) - can_sends.append(self.CCS.create_acc_07_control(self.packer_pt, CANBUS.pt, CC.longActive, - accel, acc_hold_request, acc_hold_release, - acc_hold_type, stopping_distance)) - - if self.frame % self.CCP.ACC_HUD_STEP == 0: - if hud_control.leadVisible: - lead_distance = 512 if CS.digital_cluster_installed else 8 # TODO: look up actual distance to lead - else: - lead_distance = 0 - - can_sends.append(self.CCS.create_acc_02_control(self.packer_pt, CANBUS.pt, CS.tsk_status, - hud_control.setSpeed * CV.MS_TO_KPH, lead_distance)) - can_sends.append(self.CCS.create_acc_04_control(self.packer_pt, CANBUS.pt, CS.acc_04_stock_values)) - # **** Steering Controls ************************************************ # if self.frame % self.CCP.HCA_STEP == 0: @@ -125,9 +73,9 @@ 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_status = self.CCS.acc_control_status_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)) + can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CC.longActive, acc_status, accel)) # **** HUD Controls ***************************************************** # @@ -141,8 +89,9 @@ class CarController: if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? - can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - hud_control.leadVisible)) + pass_through_data = CS.acc_04_stock_values if self.pass_through_enabled else None + can_sends.extend(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, + hud_control.leadVisible, pass_through_data)) # **** Stock ACC Button Controls **************************************** # diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index cec01f7a6..38971e5bb 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -75,6 +75,7 @@ class CarInterface(CarInterfaceBase): if disable_radar and ret.networkLocation == NetworkLocation.gateway: ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL + ret.minEnableSpeed = 5 * CV.MPH_TO_MS # FIXME: temp hack during refactor if ret.transmissionType == TransmissionType.manual: ret.minEnableSpeed = 4.5 # FIXME: estimated, fine-tune diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index d9b275c8a..ddb940569 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -38,54 +38,68 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, return packer.make_can_msg("GRA_ACC_01", bus, values) -def create_acc_02_control(packer, bus, acc_status, set_speed, lead_distance): - values = { - "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status, - "ACC_Wunschgeschw": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": 3, - "ACC_Display_Prio": 3, - "ACC_Abstandsindex": lead_distance, - } - - return packer.make_can_msg("ACC_02", bus, values) +def acc_control_status_value(main_switch_on, acc_faulted, long_active): + if acc_faulted: + tsk_status = 6 + elif long_active: + tsk_status = 3 + elif main_switch_on: + tsk_status = 2 + else: + tsk_status = 0 + return tsk_status -def create_acc_04_control(packer, bus, acc_04_stock_values): - values = acc_04_stock_values.copy() - # Suppress disengagement alert from stock radar when OP long is in use, but passthru FCW/AEB alerts - if values["ACC_Texte_braking_guard"] == 4: - values["ACC_Texte_braking_guard"] = 0 - - return packer.make_can_msg("ACC_04", bus, values) +def create_acc_accel_control(packer, bus, enabled, acc_status, accel): + commands = [] - -def create_acc_06_control(packer, bus, enabled, acc_status, accel, acc_stopping, acc_starting, cb_pos, cb_neg, acc_type): - values = { - "ACC_Typ": acc_type, + acc_06_values = { + "ACC_Typ": 2, # FIXME: require SnG during refactoring, re-enable FtS later "ACC_Status_ACC": acc_status, "ACC_StartStopp_Info": enabled, "ACC_Sollbeschleunigung_02": accel if enabled else 3.01, - "ACC_zul_Regelabw_unten": cb_neg, - "ACC_zul_Regelabw_oben": cb_pos, + "ACC_zul_Regelabw_unten": 0.1, # FIXME: reintroduce comfort band support + "ACC_zul_Regelabw_oben": 0.1, # FIXME: reintroduce comfort band support "ACC_neg_Sollbeschl_Grad_02": 4.0 if enabled else 0, "ACC_pos_Sollbeschl_Grad_02": 4.0 if enabled else 0, - "ACC_Anfahren": acc_starting, - "ACC_Anhalten": acc_stopping, + "ACC_Anfahren": 0, # FIXME: minspeed > 0 during refactor + "ACC_Anhalten": 0, # FIXME: minspeed > 0 during refactor } + commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values)) - return packer.make_can_msg("ACC_06", bus, values) - - -def create_acc_07_control(packer, bus, enabled, accel, acc_hold_request, acc_hold_release, acc_hold_type, stopping_distance): - values = { - "ACC_Distance_to_Stop": stopping_distance, - "ACC_Hold_Request": acc_hold_request, + acc_07_values = { + "ACC_Distance_to_Stop": 20.46, # FIXME: default value, minspeed > 0 during refactor + "ACC_Hold_Request": 0, # FIXME: default value, minspeed > 0 during refactor "ACC_Freewheel_Type": 2 if enabled else 0, - "ACC_Hold_Type": acc_hold_type, - "ACC_Hold_Release": acc_hold_release, + "ACC_Hold_Type": 0, # FIXME: default value, minspeed > 0 during refactor + "ACC_Hold_Release": 0, # FIXME: default value, minspeed > 0 during refactor "ACC_Accel_Secondary": 3.02, # not using this unless and until we understand its impact "ACC_Accel_TSK": accel if enabled else 3.01, } + commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values)) + + return commands + + +def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, pass_through_data): + commands = [] + + acc_02_values = { + "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status, + "ACC_Wunschgeschw": set_speed if set_speed < 250 else 327.36, + "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Display_Prio": 3, + "ACC_Abstandsindex": 512 if lead_visible else 0, # FIXME: will break analog clusters during refactor + } + commands.append(packer.make_can_msg("ACC_02", bus, acc_02_values)) + + # Suppress disengagement alert from stock radar when OP long is in use, but passthru FCW/AEB alerts + acc_04_values = pass_through_data.copy() + if acc_04_values["ACC_Texte_braking_guard"] == 4: + acc_04_values["ACC_Texte_braking_guard"] = 0 + + commands.append(packer.make_can_msg("ACC_04", bus, acc_04_values)) + + return commands - return packer.make_can_msg("ACC_07", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 1e7ed2134..8077ffe3a 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -35,7 +35,7 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, return packer.make_can_msg("GRA_Neu", bus, values) -def tsk_status_value(main_switch_on, acc_faulted, long_active): +def acc_control_status_value(main_switch_on, acc_faulted, long_active): if long_active: tsk_status = 1 elif main_switch_on: @@ -59,20 +59,20 @@ 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, enabled, acc_status, accel): values = { - "ACS_Sta_ADR": adr_status, - "ACS_StSt_Info": adr_status != 1, + "ACS_Sta_ADR": acc_status, + "ACS_StSt_Info": acc_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_Sollbeschl": accel if acc_status == 1 else 3.01, + "ACS_zul_Regelabw": 0.2 if acc_status == 1 else 1.27, + "ACS_max_AendGrad": 3.0 if acc_status == 1 else 5.08, } - return packer.make_can_msg("ACC_System", bus, values) + packer.make_can_msg("ACC_System", bus, values) -def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, pass_through_data=None): values = { "ACA_StaACC": acc_status, "ACA_Zeitluecke": 2,