Pull request for safety only changes for pre-autopilot Tesla Model S

cereal/car.capnp

@@ -302,6 +302,7 @@ struct CarParams {
     hondaBosch @5;
     ford @6;
     cadillac @7;
+    tesla @8;
   }
 
   # things about the car in the manual

panda/board/drivers/gmlanswitch.h

@@ -0,0 +1,74 @@
+#define GMLAN_TICKS_PER_SECOND 33300 //1sec @ 33.3kbps
+#define GMLAN_TICKS_PER_TIMEOUT_TICKLE 500 //15ms @ 33.3kbps
+#define GMLAN_HIGH 0 //0 is high on bus (dominant)
+#define GMLAN_LOW 1 //1 is low on bus
+
+#ifdef PANDA
+int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms
+int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second
+
+int inverted_bit_to_send = GMLAN_HIGH; 
+int gmlan_switch_enabled = -1;
+
+void TIM4_IRQHandler(void) {
+  if (TIM4->SR & TIM_SR_UIF && gmlan_switch_enabled != -1) {
+    if (can_timeout_counter == 0) {
+      //it has been more than 1 second since receiving a CAN message in tesla safety. Assume we are in a different safety mode, disable timer and restore the GMLAN output
+      set_gpio_output(GPIOB, 13, GMLAN_LOW);
+      //set_gpio_mode(GPIOB, 13, MODE_INPUT);
+      //TIM4->DIER = 0;  // no update interrupt
+      //TIM4->CR1 = 0;   // disable timer
+      gmlan_switch_enabled = -1;
+      gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
+    }
+    else {
+      can_timeout_counter--;
+      if (gmlan_timeout_counter == 0) {
+        //Send a 1 (bus low) every 15ms to reset the GMLAN transceivers timeout
+        gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
+        set_gpio_output(GPIOB, 13, GMLAN_LOW);
+      }
+      else {
+        set_gpio_output(GPIOB, 13, inverted_bit_to_send);
+        gmlan_timeout_counter--;
+      }
+    }
+  }
+  TIM4->SR = 0;
+}
+
+void gmlan_switch_init(void) {
+  gmlan_switch_enabled = 1;
+  set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
+
+  // setup
+  TIM4->PSC = 48-1;          // tick on 1 us
+  TIM4->CR1 = TIM_CR1_CEN;   // enable
+  TIM4->ARR = 30-1;          // 33.3 kbps
+
+  // in case it's disabled
+  NVIC_EnableIRQ(TIM4_IRQn);
+
+  // run the interrupt
+  TIM4->DIER = TIM_DIER_UIE; // update interrupt
+  TIM4->SR = 0;
+
+  inverted_bit_to_send = GMLAN_HIGH; //We got initialized, set the output high 
+}
+
+void set_gmlan_digital_output(int to_set) {
+  inverted_bit_to_send = to_set;
+  /*
+  puts("Writing ");
+  puth(inverted_bit_to_send);
+  puts("\n");
+  */
+}
+
+void enable_gmlan_switch(void) {
+  can_timeout_counter = GMLAN_TICKS_PER_SECOND;
+  gmlan_switch_enabled = 1; 
+  inverted_bit_to_send = GMLAN_HIGH;
+}
+
+#endif

panda/board/safety.h

@@ -49,6 +49,7 @@ int controls_allowed = 0;
 #include "safety/safety_gm.h"
 #include "safety/safety_ford.h"
 #include "safety/safety_cadillac.h"
+#include "safety/safety_tesla.h"
 #include "safety/safety_elm327.h"
 
 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -87,6 +88,7 @@ typedef struct {
 #define SAFETY_HONDA_BOSCH 4
 #define SAFETY_FORD 5
 #define SAFETY_CADILLAC 6
+#define SAFETY_TESLA 7
 #define SAFETY_TOYOTA_IPAS 0x1335
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
@@ -100,6 +102,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_GM, &gm_hooks},
   {SAFETY_FORD, &ford_hooks},
   {SAFETY_CADILLAC, &cadillac_hooks},
+  {SAFETY_TESLA, &tesla_hooks},  
   {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
 #ifdef PANDA
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},

panda/board/safety/safety_tesla.h

@@ -0,0 +1,170 @@
+#include "../drivers/gmlanswitch.h"
+
+// board enforces
+//   in-state
+//      accel set/resume
+//   out-state
+//      cancel button
+//      regen paddle
+//      accel rising edge
+//      brake rising edge
+//      brake > 0mph
+
+// lateral limits
+const int16_t MAX_ANGLE = 20; //Degrees
+
+int tesla_brake_prev = 0;
+int tesla_gas_prev = 0;
+int tesla_speed = 0;
+int current_car_time = -1;
+int time_at_last_stalk_pull = -1;
+int eac_status = 0;
+
+int tesla_ignition_started = 0;
+
+static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  enable_gmlan_switch(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled
+
+  //int bus_number = (to_push->RDTR >> 4) & 0xFF;
+  uint32_t addr;
+  if (to_push->RIR & 4) {
+    // Extended
+    // Not looked at, but have to be separated
+    // to avoid address collision
+    addr = to_push->RIR >> 3;
+  } else {
+    // Normal
+    addr = to_push->RIR >> 21;
+  }
+
+   // Record the current car time in current_car_time (for use with double-pulling cruise stalk)
+  if (addr == 0x318) {
+    int hour = (to_push->RDLR & 0x1F000000) >> 24;
+    int minute = (to_push->RDHR & 0x3F00) >> 8;
+    int second = (to_push->RDLR & 0x3F0000) >> 16;
+    current_car_time = (hour * 3600) + (minute * 60) + second;
+  }
+
+  if (addr == 0x45) {
+    // 6 bits starting at position 0
+    int lever_position = (to_push->RDLR & 0x3F);
+    if (lever_position == 2) { // pull forward
+      // activate openpilot
+      // TODO: uncomment the if to use double pull to activate
+      //if (current_car_time <= time_at_last_stalk_pull + 1 && current_car_time != -1 && time_at_last_stalk_pull != -1) {
+          controls_allowed = 1;
+      //}
+      time_at_last_stalk_pull = current_car_time;
+    } else if (lever_position == 1) { // push towards the back
+      // deactivate openpilot
+      controls_allowed = 0;
+    }
+  }  
+
+  // Detect drive rail on (ignition) (start recording)
+  if (addr == 0x348) {
+    // GTW_status
+    int drive_rail_on = (to_push->RDLR & 0x0001);
+    tesla_ignition_started = drive_rail_on == 1;
+  }
+
+  // exit controls on brake press
+  // DI_torque2::DI_brakePedal 0x118
+  //if (addr == 0x118) {
+    // 1 bit at position 16
+  //  if (((to_push->RDLR & 0x8000)) >> 15 == 1) {
+  //    controls_allowed = 0;
+  //  }
+  //}  
+
+  // exit controls on EPAS error
+  // EPAS_sysStatus::EPAS_eacStatus 0x370
+  if (addr == 0x370) {
+    // if EPAS_eacStatus is not 1 or 2, disable control
+    eac_status = ((to_push->RDHR >> 21)) & 0x7;
+    if (eac_status != 1 && eac_status != 2) {
+      controls_allowed = 0;
+    }
+  }  
+
+}
+
+// all commands: gas/regen, friction brake and steering
+// if controls_allowed and no pedals pressed
+//     allow all commands up to limit
+// else
+//     block all commands that produce actuation
+
+static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+
+  //uint32_t addr;
+  //int angle_raw;
+  //int angle_steer;
+
+  // 1 allows the message through
+  //addr = to_send->RIR >> 21;
+
+  // do not transmit CAN message if steering angle too high
+  // DAS_steeringControl::DAS_steeringAngleRequest
+  //if (addr == 0x488) {
+  //  angle_raw = to_send->RDLR & 0x7F;
+  //  angle_steer = angle_raw * 0.1 - 1638.35;
+  //  if ( (angle_steer > MAX_ANGLE) || (angle_steer < -MAX_ANGLE) ) {
+  //    return 0;
+  //  }
+  //}  
+
+  return true;
+}
+
+static int tesla_tx_lin_hook(int lin_num, uint8_t *data, int len) {
+  // LIN is not used on the Tesla
+  return false;
+}
+
+static void tesla_init(int16_t param) {
+  controls_allowed = 0;
+  tesla_ignition_started = 0;
+  gmlan_switch_init();
+}
+
+static int tesla_ign_hook() {
+  return tesla_ignition_started;
+}
+
+static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+
+  int32_t addr = to_fwd->RIR >> 21;
+
+  if (bus_num == 0) {
+
+    // change inhibit of GTW_epasControl to WITH_BOTH
+    if (addr == 0x101) {
+      to_fwd->RDLR = to_fwd->RDLR | 0xC000;
+      int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
+      to_fwd->RDLR = to_fwd->RDLR & 0xFFFF;
+      to_fwd->RDLR = to_fwd->RDLR + (checksum << 16);
+    }
+
+    return 2; // Custom EPAS bus
+  }
+  if (bus_num == 2) {
+
+    // remove GTW_epasControl in forwards
+   if (addr == 0x101) {
+     return false;
+   }
+
+    return 0; // Chassis CAN
+  }
+  return false;
+}
+
+const safety_hooks tesla_hooks = {
+  .init = tesla_init,
+  .rx = tesla_rx_hook,
+  .tx = tesla_tx_hook,
+  .tx_lin = tesla_tx_lin_hook,
+  .ignition = tesla_ign_hook,
+  .fwd = tesla_fwd_hook,
+};

panda/python/__init__.py

@@ -106,6 +106,7 @@ class Panda(object):
   SAFETY_HONDA = 1
   SAFETY_TOYOTA = 2
   SAFETY_HONDA_BOSCH = 4
+  SAFETY_TESLA = 7
   SAFETY_TOYOTA_NOLIMITS = 0x1336
   SAFETY_ALLOUTPUT = 0x1337
   SAFETY_ELM327 = 0xE327

selfdrive/boardd/boardd.cc

@@ -39,6 +39,7 @@
 #define SAFETY_HONDA_BOSCH 4
 #define SAFETY_FORD 5
 #define SAFETY_CADILLAC 6
+#define SAFETY_TESLA 7
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
 
@@ -113,6 +114,9 @@ void *safety_setter_thread(void *s) {
   case (int)cereal::CarParams::SafetyModels::CADILLAC:
     safety_setting = SAFETY_CADILLAC;
     break;
+  case (int)cereal::CarParams::SafetyModels::TESLA:
+    safety_setting = SAFETY_TESLA;
+    break;
   default:
     LOGE("unknown safety model: %d", safety_model);
   }