diff --git a/panda b/panda index 7e9a83008dac8b..78cbd8916d3cb8 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 7e9a83008dac8b590e9a6cd7e07ec2ac2b114387 +Subproject commit 78cbd8916d3cb8e631e84cb9000f4b52cfadb73a diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4331a12bf047e4..3c6719fb00679d 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -29,18 +29,32 @@ def EPB_handler(CS, self, ACS_Sta_ADR, ACS_Sollbeschl, vEgo, stopped): self.EPB_brake = ACS_Sollbeschl * 2 if stopped else ACS_Sollbeschl self.EPB_counter += 1 else: - if self.EPB_enable and self.EPB_counter < 8: # Keep EPB_enable active for 8 frames + if self.EPB_enable and self.EPB_counter < 10: # Keep EPB_enable active for 10 frames self.EPB_counter += 1 else: self.EPB_brake = 0 self.EPB_enable = 0 if CS.out.gasPressed or CS.out.brakePressed: + if self.EPB_enable: + self.ACC_anz_blind = 1 self.EPB_brake = 0 self.EPB_enable = 0 - self.EPB_enable_last = 0 + self.EPB_enable_prev = 0 + self.EPB_enable_2old = 0 - return self.EPB_enable, self.EPB_enable_last, self.EPB_brake + if self.ACC_anz_blind and self.ACC_anz_blind_counter < 150: + self.ACC_anz_blind_counter += 1 + else: + self.ACC_anz_blind = 0 + self.ACC_anz_blind_counter = 0 + + # Update EPB historical states and calculate EPB_active + self.EPB_active = int((self.EPB_enable_2old and not self.EPB_enable) or self.EPB_enable) + self.EPB_enable_2old = self.EPB_enable_prev + self.EPB_enable_prev = self.EPB_enable + + return self.EPB_enable, self.EPB_brake, self.EPB_active class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): @@ -55,6 +69,9 @@ def __init__(self, dbc_name, CP, VM): self.bremse8_counter_last = None self.bremse11_counter_last = None self.acc_sys_counter_last = None + self.acc_anz_counter_last = None + self.ACC_anz_blind = 0 + self.ACC_anz_blind_counter = 0 self.eps_timer_soft_disable_alert = False self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 @@ -64,8 +81,9 @@ def __init__(self, dbc_name, CP, VM): self.EPB_brake = 0 self.EPB_brake_last = 0 self.EPB_enable = 0 - self.EPB_enable_last = 0 - self.EPB_enabled = 0 + self.EPB_enable_prev = 0 + self.EPB_enable_2old = 0 + self.EPB_active = 0 self.EPB_counter = 0 self.accel_diff = 0 self.long_deviation = 0 @@ -282,21 +300,22 @@ def update(self, CC, CS, now_nanos): if CS.acc_sys_stock["COUNTER"] != self.acc_sys_counter_last: EPB_handler(CS, self, CS.acc_sys_stock["ACS_Sta_ADR"], CS.acc_sys_stock["ACS_Sollbeschl"], CS.out.vEgoRaw, self.stopped) - self.EPB_enabled = int((self.EPB_enable_last and not self.EPB_enable) or self.EPB_enable) - can_sends.append(self.CCS.filter_ACC_System(self.packer_pt, CANBUS.pt, CS.acc_sys_stock, self.EPB_enabled)) + can_sends.append(self.CCS.filter_ACC_System(self.packer_pt, CANBUS.pt, CS.acc_sys_stock, self.EPB_active)) can_sends.append(self.CCS.create_epb_control(self.packer_pt, CANBUS.br, self.EPB_brake, self.EPB_enable)) 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 - can_sends.append(self.CCS.filter_motor2(self.packer_pt, CANBUS.cam, CS.motor2_stock, self.EPB_enabled)) + 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_enabled)) + 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)) self.motor2_last = CS.motor2_stock self.motor2_frame += 1 - self.EPB_enable_last = self.EPB_enable 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"] self.bremse11_counter_last = CS.bremse11_stock["COUNTER"] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 9d1f5c5cec4b0c..8be04be9de3d91 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -257,6 +257,7 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): ret.cruiseState.speed = 0 self.acc_sys_stock = ext_cp.vl["ACC_System"] + self.acc_anz_stock = ext_cp.vl["ACC_GRA_Anzeige"] self.motor2_stock = pt_cp.vl["Motor_2"] self.bremse8_stock = pt_cp.vl["Bremse_8"] self.bremse11_stock = pt_cp.vl["Bremse_11"] diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index af6bc7539c804c..50d2b39ebdea3f 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -173,6 +173,15 @@ def filter_ACC_System(packer, bus, acc_car, epb_freigabe): # bus 2 --> 0 }) return packer.make_can_msg("ACC_System", bus, values) +def filter_ACC_Anzeige(packer, bus, anz_car, blind): # bus 2 --> 0 + values = anz_car + if blind: + values.update({ + "ACA_Fahrerhinw": 0, + "ACA_Akustik2": 0, + }) + return packer.make_can_msg("ACC_GRA_Anzeige", bus, values) + def create_epb_control(packer, bus, apply_brake, epb_enabled): # bus 1 values = { "EP1_Fehler_Sta": 0,