From 39d8870c00370c2528b45571248abe47558dc6f0 Mon Sep 17 00:00:00 2001 From: Robert Cotran Date: Mon, 16 Jul 2018 11:54:58 -0400 Subject: [PATCH 1/2] Changes required for safety checks on Panda for pre-autopilot Tesla Model S. --- cereal/car.capnp | 1 + panda/board/drivers/gmlanswitch.h | 1 + panda/board/safety.h | 3 + panda/board/safety/safety_tesla.h | 170 ++++++++++++++++++++++++++++++ panda/python/__init__.py | 1 + selfdrive/boardd/boardd.cc | 4 + 6 files changed, 180 insertions(+) create mode 100644 panda/board/drivers/gmlanswitch.h create mode 100644 panda/board/safety/safety_tesla.h diff --git a/cereal/car.capnp b/cereal/car.capnp index ffc79e6476dc69..ece2b92cb3a893 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -302,6 +302,7 @@ struct CarParams { hondaBosch @5; ford @6; cadillac @7; + tesla @8; } # things about the car in the manual diff --git a/panda/board/drivers/gmlanswitch.h b/panda/board/drivers/gmlanswitch.h new file mode 100644 index 00000000000000..340260d8c598c8 --- /dev/null +++ b/panda/board/drivers/gmlanswitch.h @@ -0,0 +1 @@ +#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 \ No newline at end of file diff --git a/panda/board/safety.h b/panda/board/safety.h index 4b5a84084845dd..ef9f9be8b8e735 100644 --- a/panda/board/safety.h +++ b/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}, diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h new file mode 100644 index 00000000000000..ba382ba6b6a078 --- /dev/null +++ b/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, +}; \ No newline at end of file diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 86b76285ee0ddb..80022527a22dec 100644 --- a/panda/python/__init__.py +++ b/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 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 669f0fdd8cbb5d..97cc3abe62abc3 100644 --- a/selfdrive/boardd/boardd.cc +++ b/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); } From 3be3a58187f38c09ad5f6bc24ce82e9743e49ff7 Mon Sep 17 00:00:00 2001 From: Robert Cotran Date: Mon, 16 Jul 2018 11:57:53 -0400 Subject: [PATCH 2/2] Fixed EOL for gmlanswitch.h to Unix --- panda/board/drivers/gmlanswitch.h | 75 ++++++++++++++++++++++++++++++- 1 file changed, 74 insertions(+), 1 deletion(-) diff --git a/panda/board/drivers/gmlanswitch.h b/panda/board/drivers/gmlanswitch.h index 340260d8c598c8..bf5478239a5ea9 100644 --- a/panda/board/drivers/gmlanswitch.h +++ b/panda/board/drivers/gmlanswitch.h @@ -1 +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 \ No newline at end of file +#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 \ No newline at end of file