Skip to content

Commit

Permalink
VW PQ: add Res spam while stopped
Browse files Browse the repository at this point in the history 
* pulse res for 0.5s then off for 0.5s while stopped
  • Loading branch information
@dkiiv
dkiiv committed Dec 9, 2024
1 parent 07fa48b commit c69d8ad
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 33 deletions.
2 changes: 1 addition & 1 deletion panda
Submodule panda updated from 78cbd8 to 0810bf
65 changes: 33 additions & 32 deletions selfdrive/car/volkswagen/carcontroller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ def __init__(self, dbc_name, CP, VM):
self.hca_frame_same_torque = 0
self.last_button_frame = 0
self.accel_last = 0
self.motor2_frame = 0
self.frame = 0
self.EPB_brake = 0
self.EPB_brake_last = 0
self.EPB_enable = 0
Expand Down Expand Up @@ -258,40 +258,40 @@ def update(self, CC, CS, now_nanos):
lead_distance, hud_control.leadDistanceBars))

# **** Stock ACC Button Controls **************************************** #

if CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, self.CP.openpilotLongitudinalControl,
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled:
if not self.CP.pcmCruiseSpeed:
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
if self.cruise_button is not None:
if self.acc_type == -1:
if self.button_count >= 2 and self.v_set_dis_prev != self.v_set_dis:
self.acc_type = 1 if abs(self.v_set_dis - self.v_set_dis_prev) >= 10 and self.last_cruise_button in (1, 2) else \
0 if abs(self.v_set_dis - self.v_set_dis_prev) < 10 and self.last_cruise_button not in (1, 2) else 1
if self.send_count >= 10 and self.v_set_dis_prev == self.v_set_dis:
self.cruise_button = 3 if self.cruise_button == 1 else 4
if self.acc_type == 0:
self.cruise_button = 1 if self.cruise_button == 1 else 2 # accel, decel
elif self.acc_type == 1:
self.cruise_button = 3 if self.cruise_button == 1 else 4 # resume, set
if self.frame % self.CCP.BTN_STEP == 0:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, CS.gra_stock_values, self.CP.openpilotLongitudinalControl,
frame=(self.frame // self.CCP.BTN_STEP), buttons=self.cruise_button,
custom_stock_long=True))
self.send_count += 1
else:
self.send_count = 0
self.last_cruise_button = self.cruise_button
if self.CP.openpilotLongitudinalControl:
if CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, self.CP.openpilotLongitudinalControl,
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled:
if not self.CP.pcmCruiseSpeed:
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
if self.cruise_button is not None:
if self.acc_type == -1:
if self.button_count >= 2 and self.v_set_dis_prev != self.v_set_dis:
self.acc_type = 1 if abs(self.v_set_dis - self.v_set_dis_prev) >= 10 and self.last_cruise_button in (1, 2) else \
0 if abs(self.v_set_dis - self.v_set_dis_prev) < 10 and self.last_cruise_button not in (1, 2) else 1
if self.send_count >= 10 and self.v_set_dis_prev == self.v_set_dis:
self.cruise_button = 3 if self.cruise_button == 1 else 4
if self.acc_type == 0:
self.cruise_button = 1 if self.cruise_button == 1 else 2 # accel, decel
elif self.acc_type == 1:
self.cruise_button = 3 if self.cruise_button == 1 else 4 # resume, set
if self.frame % self.CCP.BTN_STEP == 0:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, CS.gra_stock_values, self.CP.openpilotLongitudinalControl,
frame=(self.frame // self.CCP.BTN_STEP), buttons=self.cruise_button,
custom_stock_long=True))
self.send_count += 1
else:
self.send_count = 0
self.last_cruise_button = self.cruise_button

# **** Blinding Motor_2 for PQ radar ************ #
if VolkswagenFlags.PQ and self.ext_bus == CANBUS.cam and self.CP.openpilotLongitudinalControl:
if self.motor2_frame % 2 or CS.motor2_stock != getattr(self, 'motor2_last', CS.motor2_stock): # 50hz / 20ms
if self.frame % 2 or CS.motor2_stock != getattr(self, 'motor2_last', CS.motor2_stock): # 50hz / 20ms
can_sends.append(self.CCS.create_motor2_control(self.packer_pt, CANBUS.cam, CS.motor2_stock))
self.motor2_frame = 0
self.frame = 0
self.motor2_last = CS.motor2_stock
self.motor2_frame += 1
self.frame += 1

# *** Below here is for OEM+ behavior modification of OEM ACC *** #
# Modify Motor_2, Bremse_8, Bremse_11
Expand All @@ -305,15 +305,16 @@ def update(self, CC, CS, now_nanos):
can_sends.append(self.CCS.filter_epb1(self.packer_pt, CANBUS.cam, self.stopped)) # in custom module, filter the gateway fwd EPB msg
if CS.acc_anz_stock["COUNTER"] != self.acc_anz_counter_last:
can_sends.append(self.CCS.filter_ACC_Anzeige(self.packer_pt, CANBUS.pt, CS.acc_anz_stock, self.ACC_anz_blind))
if self.motor2_frame % 2 or CS.motor2_stock != getattr(self, 'motor2_last', CS.motor2_stock): # 50hz / 20ms
if self.frame % 2 or CS.motor2_stock != getattr(self, 'motor2_last', CS.motor2_stock): # 50hz / 20ms
can_sends.append(self.CCS.filter_motor2(self.packer_pt, CANBUS.cam, CS.motor2_stock, self.EPB_active))
if CS.bremse8_stock["COUNTER"] != self.bremse8_counter_last:
can_sends.append(self.CCS.filter_bremse8(self.packer_pt, CANBUS.cam, CS.bremse8_stock, self.EPB_active))
if CS.bremse11_stock["COUNTER"] != self.bremse11_counter_last:
can_sends.append(self.CCS.filter_bremse11(self.packer_pt, CANBUS.cam, CS.bremse11_stock, self.stopped))
if CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:
can_sends.append(self.CCS.filter_GRA_Neu(self.packer_pt, CANBUS.cam, CS.gra_stock_values, resume = self.stopped and (self.frame % 100 < 50)))

self.motor2_last = CS.motor2_stock
self.motor2_frame += 1
self.acc_sys_counter_last = CS.acc_sys_stock["COUNTER"]
self.acc_anz_counter_last = CS.acc_anz_stock["COUNTER"]
self.bremse8_counter_last = CS.bremse8_stock["COUNTER"]
Expand Down
8 changes: 8 additions & 0 deletions selfdrive/car/volkswagen/pqcan.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,14 @@ def filter_ACC_Anzeige(packer, bus, anz_car, blind): # bus 2 --> 0
})
return packer.make_can_msg("ACC_GRA_Anzeige", bus, values)

def filter_GRA_Neu(packer, bus, gra_car, resume): # bus 2 --> 0
values = gra_car
if resume:
values.update({
"GRA_Recall": 1,
})
return packer.make_can_msg("GRA_Neu", bus, values)

def create_epb_control(packer, bus, apply_brake, epb_enabled): # bus 1
values = {
"EP1_Fehler_Sta": 0,
Expand Down

0 comments on commit c69d8ad

Please sign in to comment.