diff --git a/panda b/panda index 78cbd8916d3cb8..0810bf6ffa4af5 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 78cbd8916d3cb8e631e84cb9000f4b52cfadb73a +Subproject commit 0810bf6ffa4af571d21d1969d29f9d9b6b7d89fe diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 3c6719fb00679d..3464f2db02582c 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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 @@ -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 @@ -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"] diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 50d2b39ebdea3f..d66143274295aa 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -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,