sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

refactor all the things

Browse Source
mqb-long
Jason Young 3 years ago
parent 5dc347f812
commit f35c28a988
6 changed files with 67 additions and 103 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      panda
  2. 2
      selfdrive/car/__init__.py
  3. 63
      selfdrive/car/volkswagen/carcontroller.py
  4. 1
      selfdrive/car/volkswagen/interface.py
  5. 84
      selfdrive/car/volkswagen/mqbcan.py
  6. 18
      selfdrive/car/volkswagen/pqcan.py

2
panda

@ -1 +1 @@
Subproject commit 0e8854b48ff81cc61e30e9135091846ab5d9b887
Subproject commit b0472024d324984d0453cc5d0ae506d58980bf91

2
selfdrive/car/__init__.py
Unescape View File

@ -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:

63
selfdrive/car/volkswagen/carcontroller.py
Unescape View File

@ -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 **************************************** #

1
selfdrive/car/volkswagen/interface.py
Unescape View File

@ -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

84
selfdrive/car/volkswagen/mqbcan.py
Unescape View File

@ -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)

18
selfdrive/car/volkswagen/pqcan.py
Unescape View File

@ -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,

Write Preview
Loading…
Cancel
Save