Run pedal on can bus 1 for a-pillar harness

rafcabezas · Dec 21, 2020 · 63a3a0b · 63a3a0b
1 parent 97f29e6
commit 63a3a0b
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 13 deletions.
diff --git 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,26 +2053,27 @@ 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
     if ((addr == 0x531) || (addr == 0x651)){ 
       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
@@ -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
@@ -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
@@ -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: