Safety for Tesla Model 3 / Model Y

board/drivers/can_common.h

@@ -173,10 +173,13 @@ void ignition_can_hook(CANPacket_t *to_push) {
       ignition_can_cnt = 0U;
     }
 
-    // Tesla exception
-    if ((addr == 0x348) && (len == 8)) {
-      // GTW_status
-      ignition_can = (GET_BYTE(to_push, 0) & 0x1U) != 0U;
+    // Tesla Model 3 exception
+    if ((addr == 0x118) && (len == 8)) {
+      // DI_state
+      int gear = GET_BYTE(to_push, 2) >> 5;
+      ignition_can = (gear == 2) ||  // DI_GEAR_R
+                     (gear == 3) ||  // DI_GEAR_N
+                     (gear == 4);    // DI_GEAR_D
       ignition_can_cnt = 0U;
     }
 

board/safety/safety_tesla.h

@@ -3,70 +3,63 @@
 #include "safety_declarations.h"
 
 static bool tesla_longitudinal = false;
-static bool tesla_powertrain = false;  // Are we the second panda intercepting the powertrain bus?
-static bool tesla_raven = false;
-
 static bool tesla_stock_aeb = false;
 
 static void tesla_rx_hook(const CANPacket_t *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
 
-  if (!tesla_powertrain) {
-    if((!tesla_raven && (addr == 0x370) && (bus == 0)) || (tesla_raven && (addr == 0x131) && (bus == 2))) {
-      // Steering angle: (0.1 * val) - 819.2 in deg.
-      // Store it 1/10 deg to match steering request
-      int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U;
-      update_sample(&angle_meas, angle_meas_new);
-    }
+  if((addr == 0x370) && (bus == 0)) {
+    // Steering angle: (0.1 * val) - 819.2 in deg.
+    // Store it 1/10 deg to match steering request
+    int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U;
+    update_sample(&angle_meas, angle_meas_new);
   }
 
   if(bus == 0) {
-    if(addr == (tesla_powertrain ? 0x116 : 0x118)) {
-      // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
-      float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
-      vehicle_moving = ABS(speed) > 0.1;
+    if(addr == 0x257){
+      // Vehicle speed: ((val * 0.08) - 40) * KPH_TO_MPS
+      float speed = (((((GET_BYTE(to_push, 2)) << 4) | (GET_BYTE(to_push, 1) >> 4)) * 0.08) - 40) * 0.277778;
       UPDATE_VEHICLE_SPEED(speed);
     }
 
-    if(addr == (tesla_powertrain ? 0x106 : 0x108)) {
-      // Gas pressed
-      gas_pressed = (GET_BYTE(to_push, 6) != 0U);
+    // Gas pressed
+    if(addr == 0x118){
+      gas_pressed = (GET_BYTE(to_push, 4) != 0U);
     }
 
-    if(addr == (tesla_powertrain ? 0x1f8 : 0x20a)) {
-      // Brake pressed
-      brake_pressed = (((GET_BYTE(to_push, 0) & 0x0CU) >> 2) != 1U);
+    // Brake pressed
+    if(addr == 0x39d){
+      brake_pressed = (GET_BYTE(to_push, 2) & 0x03U)  == 2U;
     }
 
-    if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
-      // Cruise state
-      int cruise_state = (GET_BYTE(to_push, 1) >> 4);
+    // Cruise state
+    if(addr == 0x286) {
+      int cruise_state = ((GET_BYTE(to_push, 1) << 1 ) >> 5);
       bool cruise_engaged = (cruise_state == 2) ||  // ENABLED
                             (cruise_state == 3) ||  // STANDSTILL
                             (cruise_state == 4) ||  // OVERRIDE
                             (cruise_state == 6) ||  // PRE_FAULT
                             (cruise_state == 7);    // PRE_CANCEL
+
+      vehicle_moving = cruise_state != 3; // STANDSTILL
       pcm_cruise_check(cruise_engaged);
     }
   }
 
   if (bus == 2) {
-    int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);
-    if (tesla_longitudinal && (addr == das_control_addr)) {
+    if (tesla_longitudinal && (addr == 0x2b9)) {
       // "AEB_ACTIVE"
       tesla_stock_aeb = ((GET_BYTE(to_push, 2) & 0x03U) == 1U);
     }
   }
 
-  if (tesla_powertrain) {
-    // 0x2bf: DAS_control should not be received on bus 0
-    generic_rx_checks((addr == 0x2bf) && (bus == 0));
-  } else {
-    // 0x488: DAS_steeringControl should not be received on bus 0
-    generic_rx_checks((addr == 0x488) && (bus == 0));
-  }
+  generic_rx_checks((addr == 0x488) && (bus == 0));
+  generic_rx_checks((addr == 0x27d) && (bus == 0));
 
+  if (tesla_longitudinal) {
+      generic_rx_checks((addr == 0x2b9) && (bus == 0));
+  }
 }
 
 
@@ -84,16 +77,16 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
   };
 
   const LongitudinalLimits TESLA_LONG_LIMITS = {
-    .max_accel = 425,       // 2. m/s^2
-    .min_accel = 287,       // -3.52 m/s^2  // TODO: limit to -3.48
+    .max_accel = 425,       // 2 m/s^2
+    .min_accel = 288,       // -3.48 m/s^2
     .inactive_accel = 375,  // 0. m/s^2
   };
 
   bool tx = true;
   int addr = GET_ADDR(to_send);
   bool violation = false;
 
-  if(!tesla_powertrain && (addr == 0x488)) {
+  if(addr == 0x488) {
     // Steering control: (0.1 * val) - 1638.35 in deg.
     // We use 1/10 deg as a unit here
     int raw_angle_can = (((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1));
@@ -107,16 +100,9 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
     }
   }
 
-  if (!tesla_powertrain && (addr == 0x45)) {
-    // No button other than cancel can be sent by us
-    int control_lever_status = (GET_BYTE(to_send, 0) & 0x3FU);
-    if (control_lever_status != 1) {
-      violation = true;
-    }
-  }
-
-  if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) {
+  if(addr == 0x2b9) {
     // DAS_control: longitudinal control message
+    int acc_state = ((GET_BYTE(to_send, 1) & 0xF0U) >> 4);
     if (tesla_longitudinal) {
       // No AEB events may be sent by openpilot
       int aeb_event = GET_BYTE(to_send, 2) & 0x03U;
@@ -132,8 +118,16 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
       // Don't allow any acceleration limits above the safety limits
       int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
       int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
+
+      // Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill
+      if ((raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel)){
+        violation = true;
+      }
+
       violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
       violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
+    } else if(acc_state == 13) {
+      // Allow to cancel if not using openpilot longitudinal
     } else {
       violation = true;
     }
@@ -150,20 +144,21 @@ static int tesla_fwd_hook(int bus_num, int addr) {
   int bus_fwd = -1;
 
   if(bus_num == 0) {
-    // Chassis/PT to autopilot
+    // Party to autopilot
     bus_fwd = 2;
   }
 
   if(bus_num == 2) {
-    // Autopilot to chassis/PT
-    int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);
-
     bool block_msg = false;
-    if (!tesla_powertrain && (addr == 0x488)) {
+    if (addr == 0x488) {
+      block_msg = true;
+    }
+
+    if (addr == 0x27d) {
       block_msg = true;
     }
 
-    if (tesla_longitudinal && (addr == das_control_addr) && !tesla_stock_aeb) {
+    if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
       block_msg = true;
     }
 
@@ -176,63 +171,31 @@ static int tesla_fwd_hook(int bus_num, int addr) {
 }
 
 static safety_config tesla_init(uint16_t param) {
-  const int TESLA_FLAG_POWERTRAIN = 1;
-  const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2;
-  const int TESLA_FLAG_RAVEN = 4;
+  const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
 
-  static const CanMsg TESLA_TX_MSGS[] = {
+  static const CanMsg TESLA_M3_Y_TX_MSGS[] = {
     {0x488, 0, 4},  // DAS_steeringControl
-    {0x45, 0, 8},   // STW_ACTN_RQ
-    {0x45, 2, 8},   // STW_ACTN_RQ
     {0x2b9, 0, 8},  // DAS_control
+    {0x27D, 0, 3},  // APS_eacMonitor
   };
 
-  static const CanMsg TESLA_PT_TX_MSGS[] = {
-    {0x2bf, 0, 8},  // DAS_control
-  };
-
-  tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN);
   tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
-  tesla_raven = GET_FLAG(param, TESLA_FLAG_RAVEN);
 
   tesla_stock_aeb = false;
 
   safety_config ret;
-  if (tesla_powertrain) {
-    static RxCheck tesla_pt_rx_checks[] = {
-      {.msg = {{0x106, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque1
-      {.msg = {{0x116, 0, 6, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque2
-      {.msg = {{0x1f8, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // BrakeMessage
-      {.msg = {{0x2bf, 2, 8, .frequency = 25U}, { 0 }, { 0 }}},   // DAS_control
-      {.msg = {{0x256, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // DI_state
-    };
-
-    ret = BUILD_SAFETY_CFG(tesla_pt_rx_checks, TESLA_PT_TX_MSGS);
-  } else if (tesla_raven) {
-    static RxCheck tesla_raven_rx_checks[] = {
-      {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}},   // DAS_control
-      {.msg = {{0x131, 2, 8, .frequency = 100U}, { 0 }, { 0 }}},  // EPAS3P_sysStatus
-      {.msg = {{0x108, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque1
-      {.msg = {{0x118, 0, 6, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque2
-      {.msg = {{0x20a, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // BrakeMessage
-      {.msg = {{0x368, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // DI_state
-      {.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // GTW_carState
-    };
-
-    ret = BUILD_SAFETY_CFG(tesla_raven_rx_checks, TESLA_TX_MSGS);
-  } else {
-    static RxCheck tesla_rx_checks[] = {
-      {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}},   // DAS_control
-      {.msg = {{0x370, 0, 8, .frequency = 25U}, { 0 }, { 0 }}},   // EPAS_sysStatus
-      {.msg = {{0x108, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque1
-      {.msg = {{0x118, 0, 6, .frequency = 100U}, { 0 }, { 0 }}},  // DI_torque2
-      {.msg = {{0x20a, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // BrakeMessage
-      {.msg = {{0x368, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // DI_state
-      {.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // GTW_carState
-    };
-
-    ret = BUILD_SAFETY_CFG(tesla_rx_checks, TESLA_TX_MSGS);
-  }
+
+  static RxCheck tesla_model3_y_rx_checks[] = {
+    {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}},   // DAS_control
+    {.msg = {{0x257, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // DI_speed (speed in kph)
+    {.msg = {{0x370, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // EPAS3S_internalSAS (steering angle)
+    {.msg = {{0x118, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // DI_systemStatus (gas pedal)
+    {.msg = {{0x39d, 0, 5, .frequency = 25U}, { 0 }, { 0 }}},   // IBST_status (brakes)
+    {.msg = {{0x286, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},   // DI_state (acc state)
+    {.msg = {{0x311, 0, 7, .frequency = 10U}, { 0 }, { 0 }}},   // UI_warning (blinkers, buckle switch & doors)
+  };
+
+  ret = BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS);
   return ret;
 }
 

python/__init__.py

@@ -203,9 +203,7 @@ class Panda:
   FLAG_HYUNDAI_ALT_LIMITS = 64
   FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128
 
-  FLAG_TESLA_POWERTRAIN = 1
-  FLAG_TESLA_LONG_CONTROL = 2
-  FLAG_TESLA_RAVEN = 4
+  FLAG_TESLA_LONG_CONTROL = 1
 
   FLAG_VOLKSWAGEN_LONG_CONTROL = 1
 

tests/safety/common.py

@@ -950,9 +950,9 @@ def test_not_allow_user_brake_when_moving(self, _user_brake_msg=None, get_brake_
       _user_brake_msg = self._user_brake_msg
 
     # Brake was already pressed
+    self._rx(self._vehicle_moving_msg(self.STANDSTILL_THRESHOLD))
     self._rx(_user_brake_msg(1))
     self.safety.set_controls_allowed(1)
-    self._rx(self._vehicle_moving_msg(self.STANDSTILL_THRESHOLD))
     self._rx(_user_brake_msg(1))
     self.assertTrue(self.safety.get_controls_allowed())
     self.assertTrue(self.safety.get_longitudinal_allowed())