diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 646e367024e2f84..51aeb68045d8e33 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -83,6 +83,7 @@ char radar_VIN[] = " "; //leave empty if your radar VIN matches int tesla_radar_vin_complete = 0; uint8_t tesla_radar_can = 1; uint8_t tesla_epas_can = 2; +#define tesla_pedal_can (DAS_usesApillarHarness ? 1 : 2) int tesla_radar_trigger_message_id = 0; //not used by tesla, to showcase for other cars int radarPosition = 0; //0 nosecone, 1 facelift int radarEpasType = 0; //0/1 bosch, 2-4 mando @@ -1060,7 +1061,7 @@ static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) } //let's see if the pedal was pressed - if ((addr == 0x552) && (bus_number == tesla_epas_can)) { + if ((addr == 0x552) && (bus_number == tesla_pedal_can)) { //m1 = 0.050796813 //m2 = 0.101593626 //d = -22.85856576 @@ -1980,7 +1981,7 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) } //first let's deal with the messages we need to send to radar - if ((bus_num == 0) || ((bus_num == 2 ) && (DAS_usesApillarHarness == 1))) + if ((bus_num == 0) || ((bus_num == 2) && (DAS_usesApillarHarness == 1))) { //compute return value; do not forward 0->2 and 2->0 if no epas harness @@ -2052,6 +2053,13 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) return ret_val; } + // Don't forward pedal messages + if (bus_num == tesla_pedal_can) { + if ((addr == 0x551) || (addr == 0x552)) { + return -1; + } + } + //now let's deal with CAN1 - Radar if (bus_num == tesla_radar_can) { //send radar 0x531 and 0x651 from Radar CAN to CAN0 @@ -2059,19 +2067,13 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) return 0; } - //block everything else from radar + // don't forward anything else return -1; } //now let's deal with CAN2 if ((bus_num == tesla_epas_can) && (bus_num > 0)) { - - // remove Pedal in forwards - if ((addr == 0x551) || (addr == 0x552)) { - return -1; - } - //forward everything else to CAN 0 unless claiming no harness return 0; } diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 1008dbd65a048f3..da92ee98ba64f9a 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -233,6 +233,8 @@ def update_stat(self, CS, frame): pedalcan = 2 if CS.useWithoutHarness: pedalcan = 0 + elif CS.usesApillarHarness: + pedalcan = 1 can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx, pedalcan)) return can_sends diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index eee928ee78b524d..9c1696745272b32 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -609,6 +609,7 @@ def update(self, enabled, CS, frame, actuators, \ # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) + if (not self.PCC.pcc_available) and frame % 5 == 0: # acc processed at 20Hz cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms * CV.MS_TO_KPH, @@ -625,6 +626,8 @@ def update(self, enabled, CS, frame, actuators, \ pedalcan = 2 if CS.useWithoutHarness: pedalcan = 0 + elif CS.usesApillarHarness: + pedalcan = 1 apply_accel, accel_needed, accel_idx = self.PCC.update_pdl( enabled, CS, diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 03dbfaad0e6de37..a9739c2b2dbadb3 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -44,11 +44,14 @@ def __init__(self, CP, CarController, CarState): self.epas_cp = None self.pedal_cp = None if self.CS.useWithoutHarness: - self.epas_cp = self.CS.get_epas_parser(CP,0) - self.pedal_cp = self.CS.get_pedal_parser(CP,0) + self.epas_cp = self.CS.get_epas_parser(CP, 0) + self.pedal_cp = self.CS.get_pedal_parser(CP, 0) else: - self.epas_cp = self.CS.get_epas_parser(CP,2) - self.pedal_cp = self.CS.get_pedal_parser(CP,2) + self.epas_cp = self.CS.get_epas_parser(CP, 2) + if self.CS.usesApillarHarness: + self.pedal_cp = self.CS.get_pedal_parser(CP, 1) + else: + self.pedal_cp = self.CS.get_pedal_parser(CP, 2) self.CC = None if CarController is not None: