mirror of
https://github.com/infiniteCable2/panda.git
synced 2026-02-18 17:23:52 +08:00
Safety for Tesla Model 3 / Model Y (#2036)
* 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 <gregjhogan@gmail.com> Co-authored-by: Shane Smiskol <shane@smiskol.com>
This commit is contained in:
Lukas
@@ -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/ && \
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -84,8 +78,8 @@ 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
|
||||
};
|
||||
|
||||
@@ -93,10 +87,10 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
|
||||
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
|
||||
@@ -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) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// Don't send messages when the stock AEB system is active
|
||||
if (tesla_stock_aeb) {
|
||||
violation = true;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
};
|
||||
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_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;
|
||||
return BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS);
|
||||
}
|
||||
|
||||
const safety_hooks tesla_hooks = {
|
||||
|
||||
@@ -1,51 +1,68 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.tesla.values import TeslaSafetyFlags
|
||||
from opendbc.safety import Safety
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.libsafety import libsafety_py
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
MAX_ACCEL = 2.0
|
||||
MIN_ACCEL = -3.5
|
||||
MSG_DAS_steeringControl = 0x488
|
||||
MSG_APS_eacMonitor = 0x27d
|
||||
MSG_DAS_Control = 0x2b9
|
||||
|
||||
|
||||
class CONTROL_LEVER_STATE:
|
||||
DN_1ST = 32
|
||||
UP_1ST = 16
|
||||
DN_2ND = 8
|
||||
UP_2ND = 4
|
||||
RWD = 2
|
||||
FWD = 1
|
||||
IDLE = 0
|
||||
class TestTeslaSafetyBase(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest, common.LongitudinalAccelSafetyTest):
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (MSG_DAS_steeringControl, MSG_APS_eacMonitor)}
|
||||
FWD_BLACKLISTED_ADDRS = {2: [MSG_DAS_steeringControl, MSG_APS_eacMonitor]}
|
||||
TX_MSGS = [[MSG_DAS_steeringControl, 0], [MSG_APS_eacMonitor, 0], [MSG_DAS_Control, 0]]
|
||||
|
||||
|
||||
class TestTeslaSafety(common.PandaCarSafetyTest):
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
STANDSTILL_THRESHOLD = 0.1
|
||||
GAS_PRESSED_THRESHOLD = 3
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = None
|
||||
raise unittest.SkipTest
|
||||
# Angle control limits
|
||||
DEG_TO_CAN = 10
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
values = {"DI_vehicleSpeed": speed / 0.447}
|
||||
return self.packer.make_can_msg_panda("DI_torque2", 0, values)
|
||||
ANGLE_RATE_BP = [0., 5., 15.]
|
||||
ANGLE_RATE_UP = [10., 1.6, .3] # windup limit
|
||||
ANGLE_RATE_DOWN = [10., 7.0, .8] # unwind limit
|
||||
|
||||
# Long control limits
|
||||
MAX_ACCEL = 2.0
|
||||
MIN_ACCEL = -3.48
|
||||
INACTIVE_ACCEL = 0.0
|
||||
|
||||
packer: CANPackerPanda
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "TestTeslaSafetyBase":
|
||||
raise unittest.SkipTest
|
||||
|
||||
def _angle_cmd_msg(self, angle: float, enabled: bool):
|
||||
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0}
|
||||
return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values)
|
||||
|
||||
def _angle_meas_msg(self, angle: float):
|
||||
values = {"EPAS3S_internalSAS": angle}
|
||||
return self.packer.make_can_msg_panda("EPAS3S_sysStatus", 0, values)
|
||||
|
||||
def _user_brake_msg(self, brake):
|
||||
values = {"driverBrakeStatus": 2 if brake else 1}
|
||||
return self.packer.make_can_msg_panda("BrakeMessage", 0, values)
|
||||
values = {"IBST_driverBrakeApply": 2 if brake else 1}
|
||||
return self.packer.make_can_msg_panda("IBST_status", 0, values)
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
values = {"DI_vehicleSpeed": speed * 3.6}
|
||||
return self.packer.make_can_msg_panda("DI_speed", 0, values)
|
||||
|
||||
def _vehicle_moving_msg(self, speed: float):
|
||||
values = {"DI_cruiseState": 3 if speed <= self.STANDSTILL_THRESHOLD else 2}
|
||||
return self.packer.make_can_msg_panda("DI_state", 0, values)
|
||||
|
||||
def _user_gas_msg(self, gas):
|
||||
values = {"DI_pedalPos": gas}
|
||||
return self.packer.make_can_msg_panda("DI_torque1", 0, values)
|
||||
|
||||
def _control_lever_cmd(self, command):
|
||||
values = {"SpdCtrlLvr_Stat": command}
|
||||
return self.packer.make_can_msg_panda("STW_ACTN_RQ", 0, values)
|
||||
values = {"DI_accelPedalPos": gas}
|
||||
return self.packer.make_can_msg_panda("DI_systemStatus", 0, values)
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
values = {"DI_cruiseState": 2 if enable else 0}
|
||||
@@ -63,68 +80,49 @@ class TestTeslaSafety(common.PandaCarSafetyTest):
|
||||
}
|
||||
return self.packer.make_can_msg_panda("DAS_control", bus, values)
|
||||
|
||||
def _accel_msg(self, accel: float):
|
||||
# For common.LongitudinalAccelSafetyTest
|
||||
return self._long_control_msg(10, accel_limits=(accel, max(accel, 0)))
|
||||
|
||||
class TestTeslaSteeringSafety(TestTeslaSafety, common.AngleSteeringSafetyTest):
|
||||
TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2]]
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0x488,)}
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0x488]}
|
||||
def test_vehicle_speed_measurements(self):
|
||||
# OVERRIDDEN: 79.1667 is the max speed in m/s
|
||||
self._common_measurement_test(self._speed_msg, 0, 285 / 3.6, common.VEHICLE_SPEED_FACTOR,
|
||||
self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max)
|
||||
|
||||
# Angle control limits
|
||||
DEG_TO_CAN = 10
|
||||
|
||||
ANGLE_RATE_BP = [0., 5., 15.]
|
||||
ANGLE_RATE_UP = [10., 1.6, .3] # windup limit
|
||||
ANGLE_RATE_DOWN = [10., 7.0, .8] # unwind limit
|
||||
class TestTeslaStockSafety(TestTeslaSafetyBase):
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("tesla_can")
|
||||
self.packer = CANPackerPanda("tesla_model3_party")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(Safety.SAFETY_TESLA, 0)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _angle_cmd_msg(self, angle: float, enabled: bool):
|
||||
values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": 1 if enabled else 0}
|
||||
return self.packer.make_can_msg_panda("DAS_steeringControl", 0, values)
|
||||
def test_accel_actuation_limits(self, stock_longitudinal=True):
|
||||
super().test_accel_actuation_limits(stock_longitudinal)
|
||||
|
||||
def _angle_meas_msg(self, angle: float):
|
||||
values = {"EPAS_internalSAS": angle}
|
||||
return self.packer.make_can_msg_panda("EPAS_sysStatus", 0, values)
|
||||
def test_cancel(self):
|
||||
for accval in range(16):
|
||||
self.safety.set_controls_allowed(True)
|
||||
should_tx = accval == 13 # ACC_CANCEL_GENERIC_SILENT
|
||||
self.assertFalse(self._tx(self._long_control_msg(0, acc_val=accval, accel_limits=(self.MIN_ACCEL, self.MAX_ACCEL))))
|
||||
self.assertEqual(should_tx, self._tx(self._long_control_msg(0, acc_val=accval)))
|
||||
|
||||
def test_acc_buttons(self):
|
||||
"""
|
||||
FWD (cancel) always allowed.
|
||||
"""
|
||||
btns = [
|
||||
(CONTROL_LEVER_STATE.FWD, True),
|
||||
(CONTROL_LEVER_STATE.RWD, False),
|
||||
(CONTROL_LEVER_STATE.UP_1ST, False),
|
||||
(CONTROL_LEVER_STATE.UP_2ND, False),
|
||||
(CONTROL_LEVER_STATE.DN_1ST, False),
|
||||
(CONTROL_LEVER_STATE.DN_2ND, False),
|
||||
(CONTROL_LEVER_STATE.IDLE, False),
|
||||
]
|
||||
for btn, should_tx in btns:
|
||||
for controls_allowed in (True, False):
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
tx = self._tx(self._control_lever_cmd(btn))
|
||||
self.assertEqual(tx, should_tx)
|
||||
def test_no_aeb(self):
|
||||
for aeb_event in range(4):
|
||||
self.assertEqual(self._tx(self._long_control_msg(10, acc_val=13, aeb_event=aeb_event)), aeb_event == 0)
|
||||
|
||||
|
||||
class TestTeslaRavenSteeringSafety(TestTeslaSteeringSafety):
|
||||
class TestTeslaLongitudinalSafety(TestTeslaSafetyBase):
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (MSG_DAS_steeringControl, MSG_APS_eacMonitor, MSG_DAS_Control)}
|
||||
FWD_BLACKLISTED_ADDRS = {2: [MSG_DAS_steeringControl, MSG_APS_eacMonitor, MSG_DAS_Control]}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("tesla_can")
|
||||
self.packer = CANPackerPanda("tesla_model3_party")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(Safety.SAFETY_TESLA, TeslaSafetyFlags.FLAG_TESLA_RAVEN)
|
||||
self.safety.set_safety_hooks(Safety.SAFETY_TESLA, TeslaSafetyFlags.FLAG_TESLA_LONG_CONTROL)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _angle_meas_msg(self, angle: float):
|
||||
values = {"EPAS_internalSAS": angle}
|
||||
return self.packer.make_can_msg_panda("EPAS3P_sysStatus", 2, values)
|
||||
|
||||
class TestTeslaLongitudinalSafety(TestTeslaSafety):
|
||||
def setUp(self):
|
||||
raise unittest.SkipTest
|
||||
|
||||
def test_no_aeb(self):
|
||||
for aeb_event in range(4):
|
||||
self.assertEqual(self._tx(self._long_control_msg(10, aeb_event=aeb_event)), aeb_event == 0)
|
||||
@@ -137,50 +135,34 @@ class TestTeslaLongitudinalSafety(TestTeslaSafety):
|
||||
# stock system sends no AEB -> no forwarding, and OP is allowed to TX
|
||||
self.assertEqual(1, self._rx(no_aeb_msg_cam))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(2, no_aeb_msg_cam.addr))
|
||||
self.assertEqual(True, self._tx(no_aeb_msg))
|
||||
self.assertTrue(self._tx(no_aeb_msg))
|
||||
|
||||
# stock system sends AEB -> forwarding, and OP is not allowed to TX
|
||||
self.assertEqual(1, self._rx(aeb_msg_cam))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, aeb_msg_cam.addr))
|
||||
self.assertEqual(False, self._tx(no_aeb_msg))
|
||||
self.assertFalse(self._tx(no_aeb_msg))
|
||||
|
||||
def test_acc_accel_limits(self):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
for min_accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1):
|
||||
for max_accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1):
|
||||
# floats might not hit exact boundary conditions without rounding
|
||||
min_accel = round(min_accel, 2)
|
||||
max_accel = round(max_accel, 2)
|
||||
if controls_allowed:
|
||||
send = (MIN_ACCEL <= min_accel <= MAX_ACCEL) and (MIN_ACCEL <= max_accel <= MAX_ACCEL)
|
||||
else:
|
||||
send = np.all(np.isclose([min_accel, max_accel], 0, atol=0.0001))
|
||||
self.assertEqual(send, self._tx(self._long_control_msg(10, acc_val=4, accel_limits=[min_accel, max_accel])))
|
||||
def test_prevent_reverse(self):
|
||||
# Note: Tesla can reverse while at a standstill if both accel_min and accel_max are negative.
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
# accel_min and accel_max are positive
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(1.1, 0.8))))
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(1.1, 0.8))))
|
||||
|
||||
class TestTeslaChassisLongitudinalSafety(TestTeslaLongitudinalSafety):
|
||||
TX_MSGS = [[0x488, 0], [0x45, 0], [0x45, 2], [0x2B9, 0]]
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0x488,)}
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0x2B9, 0x488]}
|
||||
# accel_min and accel_max are both zero
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(0, 0))))
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, 0))))
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("tesla_can")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(Safety.SAFETY_TESLA, TeslaSafetyFlags.FLAG_TESLA_LONG_CONTROL)
|
||||
self.safety.init_tests()
|
||||
# accel_min and accel_max have opposing signs
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-0.8, 1.3))))
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0.8, -1.3))))
|
||||
self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, -1.3))))
|
||||
|
||||
|
||||
class TestTeslaPTLongitudinalSafety(TestTeslaLongitudinalSafety):
|
||||
TX_MSGS = [[0x2BF, 0]]
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0x2BF,)}
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0x2BF]}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("tesla_powertrain")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(Safety.SAFETY_TESLA, TeslaSafetyFlags.FLAG_TESLA_LONG_CONTROL | TeslaSafetyFlags.FLAG_TESLA_POWERTRAIN)
|
||||
self.safety.init_tests()
|
||||
# accel_min and accel_max are negative
|
||||
self.assertFalse(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-1.1, -0.6))))
|
||||
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.6, -1.1))))
|
||||
self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.1, -0.1))))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
Reference in New Issue
Block a user