Skip to content

Commit

Permalink
Safety for Tesla Model 3 / Model Y (#2036)
Browse files Browse the repository at this point in the history 
* wip model3

* master

* tesla model 3 / y

* prevent tesla to reverse

* remove can 1 from safety

* use DI_vehicleSpeed

* - add APS_eacMonitor to TX
- use DI_state as a standstill signal

* block eacMonitor

* fix tesla safety tests

* fix tesla safety tests

* add generic_rx_check for eacMonitor

* fix tests

* consistent ordering of common user brake test setup

* Tesla: Panda safety update (#2075)

* - match "vehicle_moving" with opendbc
- allow to cancel

* remove comment

* update _vehicle_moving_msg

* remove redundant condition

* whoops

* update ref

* spacing!

* long behind ALLOW_DEBUG

* consistent styling

* ?

* misra and clean up

* divide instead

* double (())

* more stylistic

* this is more clear

* always check aeb

* this test catches it

* it should test angle steering in both modes?

* we weren't testing long at all, and ALSO PYTEST SILENTLY SKIPS CLASSES WITH MISSING ABSTRACT METHODS WTF

* finalize safety tests

* update opendbc to master

* ltl

* revert this

* rm conftest

* loop this

---------

Co-authored-by: Greg Hogan <[email protected]>
Co-authored-by: Shane Smiskol <[email protected]>
  • Loading branch information
@lukasloetkolben @gregjhogan @sshane
3 people authored Feb 18, 2025
1 parent b82c327 commit ebdc376
Show file tree
Hide file tree
Showing 3 changed files with 174 additions and 223 deletions.
2 changes: 1 addition & 1 deletion Dockerfile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ RUN pip3 install --break-system-packages --no-cache-dir $PYTHONPATH/panda/[dev]

# TODO: this should be a "pip install" or not even in this repo at all
RUN git config --global --add safe.directory $PYTHONPATH/panda
ENV OPENDBC_REF="950e7b34efa64d2ad41df3300652661fbae06f57"
ENV OPENDBC_REF="87a51e38b53d91075419f01b4cd2e625ee7d4516"
RUN cd /tmp/ && \
git clone --depth 1 https://github.com/commaai/opendbc opendbc_repo && \
cd opendbc_repo && git fetch origin $OPENDBC_REF && git checkout FETCH_HEAD && rm -rf .git/ && \
Expand Down
187 changes: 78 additions & 109 deletions board/safety/safety_tesla.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,70 +3,64 @@
#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.
if (bus == 0) {
// Steering angle: (0.1 * val) - 819.2 in deg.
if (addr == 0x370) {
// 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;
// Vehicle speed
if (addr == 0x257) {
// Vehicle speed: ((val * 0.08) - 40) / MS_TO_KPH
float speed = ((((GET_BYTE(to_push, 2) << 4) | (GET_BYTE(to_push, 1) >> 4)) * 0.08) - 40) / 3.6;
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) >> 4) & 0x07U;
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);
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)); // DAS_steeringControl
generic_rx_checks((addr == 0x27d) && (bus == 0)); // APS_eacMonitor

if (tesla_longitudinal) {
generic_rx_checks((addr == 0x2b9) && (bus == 0));
}
}


Expand All @@ -84,19 +78,19 @@ 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)) {
// Steering control: (0.1 * val) - 1638.35 in deg.
// Steering control: (0.1 * val) - 1638.35 in deg.
if (addr == 0x488) {
// 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));
int raw_angle_can = ((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1);
int desired_angle = raw_angle_can - 16384;
int steer_control_type = GET_BYTE(to_send, 2) >> 6;
bool steer_control_enabled = (steer_control_type != 0) && // NONE
Expand All @@ -107,35 +101,43 @@ 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) {
// DAS_control: longitudinal control message
if (addr == 0x2b9) {
// No AEB events may be sent by openpilot
int aeb_event = GET_BYTE(to_send, 2) & 0x03U;
if (aeb_event != 0) {
violation = true;
}
}

if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) {
// DAS_control: longitudinal control message
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);
int acc_state = GET_BYTE(to_send, 1) >> 4;

if (tesla_longitudinal) {
// No AEB events may be sent by openpilot
int aeb_event = GET_BYTE(to_send, 2) & 0x03U;
if (aeb_event != 0) {
// Don't send messages when the stock AEB system is active
if (tesla_stock_aeb) {
violation = true;
}

// Don't send messages when the stock AEB system is active
if (tesla_stock_aeb) {
// 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;
}

// 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);
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
} else {
violation = true;
// does allowing cancel here disrupt stock AEB? TODO: find out and add safety or remove comment
// Can only send cancel longitudinal messages when not controlling longitudinal
if (acc_state != 13) { // ACC_CANCEL_GENERIC_SILENT
violation = true;
}

// No actuation is allowed when not controlling longitudinal
if ((raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) || (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel)) {
violation = true;
}
}
}

Expand All @@ -149,25 +151,24 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
static int tesla_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;

if(bus_num == 0) {
// Chassis/PT to autopilot
if (bus_num == 0) {
// Party to autopilot
bus_fwd = 2;
}

if(bus_num == 2) {
// Autopilot to chassis/PT
int das_control_addr = (tesla_powertrain ? 0x2bf : 0x2b9);

if (bus_num == 2) {
bool block_msg = false;
if (!tesla_powertrain && (addr == 0x488)) {
// DAS_steeringControl, APS_eacMonitor
if ((addr == 0x488) || (addr == 0x27d)) {
block_msg = true;
}

if (tesla_longitudinal && (addr == das_control_addr) && !tesla_stock_aeb) {
// DAS_control
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
block_msg = true;
}

if(!block_msg) {
if (!block_msg) {
bus_fwd = 0;
}
}
Expand All @@ -176,64 +177,32 @@ 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;

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);
UNUSED(param);
#ifdef ALLOW_DEBUG
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
tesla_raven = GET_FLAG(param, TESLA_FLAG_RAVEN);
#endif

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);
}
return ret;
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)
};

return BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS);
}

const safety_hooks tesla_hooks = {
Expand Down
Loading

0 comments on commit ebdc376

Please sign in to comment.