Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tesla: Car Port for Model 3 and Model Y #29

Draft
wants to merge 4 commits into
base: master
Choose a base branch
from
Draft

Tesla: Car Port for Model 3 and Model Y #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
045ff88
Tesla: Car Port for Model 3 and Model Y
sunnyhaibin Jun 29, 2024
f8084fe
Merge branch 'master' into SP-165-tesla-port
sunnyhaibin Jul 31, 2024
165da76
MADS? MADS!
sunnyhaibin Aug 2, 2024
fe72dda
Merge remote-tracking branch 'sunnypilot/panda/master' into SP-165-te…
sunnyhaibin Aug 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 7 additions & 4 deletions board/drivers/can_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,10 +208,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;
}

Expand Down
139 changes: 51 additions & 88 deletions board/safety/safety_tesla.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,90 +16,62 @@ const LongitudinalLimits TESLA_LONG_LIMITS = {
.inactive_accel = 375, // 0. m/s^2
};


const int TESLA_FLAG_POWERTRAIN = 1;
const int TESLA_FLAG_MODEL3_Y = 1;
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2;
const int TESLA_FLAG_RAVEN = 4;

const CanMsg TESLA_TX_MSGS[] = {
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
{0x229, 1, 3}, // SCCM_rightStalk
};

const CanMsg TESLA_PT_TX_MSGS[] = {
{0x2bf, 0, 8}, // DAS_control
};

RxCheck tesla_rx_checks[] = {
RxCheck tesla_model3_y_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
};

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
};

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
{.msg = {{0x155, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // ESP_wheelRotation (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 (buckle switch & doors)
{.msg = {{0x3f5, 1, 8, .frequency = 10U}, { 0 }, { 0 }}}, // ID3F5VCFRONT_lighting (blinkers)
{.msg = {{0x229, 1, 3, .frequency = 10U}, { 0 }, { 0 }}}, // SCCM_rightStalk (right control lever)
};

bool tesla_longitudinal = false;
bool tesla_powertrain = false; // Are we the second panda intercepting the powertrain bus?
bool tesla_raven = false;

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;
if(addr == 0x155){
// Vehicle speed: (val * 0.5) * KPH_TO_MPS
float speed = ((((GET_BYTE(to_push, 6) & 0x0FU) << 6) | (GET_BYTE(to_push, 5) >> 2)) * 0.5) * 0.277778;
vehicle_moving = ABS(speed) > 0.1;
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
Expand All @@ -109,21 +81,23 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
}
}

if (bus == 1) {
if (addr == 0x229) {
int sccm_right_stalk = (GET_BYTE(to_push, 1) >> 4) & 0x7U;
bool mads_enabled = sccm_right_stalk == 3;
mads_lkas_button_check(mads_enabled);
}
}

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 == 0x2b9) && (bus == 0));

}

Expand All @@ -133,7 +107,7 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
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));
Expand All @@ -147,15 +121,16 @@ 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) {

if (addr == 0x229){
// Only the "Half up" and "Neutral" positions are permitted for sending stalk signals.
int control_lever_status = ((GET_BYTE(to_send, 1) & 0x70U) >> 4);
if ((control_lever_status > 1)) {
violation = true;
}
}

if(addr == (tesla_powertrain ? 0x2bf : 0x2b9)) {
if(addr == 0x2b9) {
// DAS_control: longitudinal control message
if (tesla_longitudinal) {
// No AEB events may be sent by openpilot
Expand Down Expand Up @@ -190,20 +165,17 @@ 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 (tesla_longitudinal && (addr == das_control_addr) && !tesla_stock_aeb) {
if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
block_msg = true;
}

Expand All @@ -216,20 +188,11 @@ static int tesla_fwd_hook(int bus_num, int addr) {
}

static safety_config tesla_init(uint16_t param) {
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) {
ret = BUILD_SAFETY_CFG(tesla_pt_rx_checks, TESLA_PT_TX_MSGS);
} else if (tesla_raven) {
ret = BUILD_SAFETY_CFG(tesla_raven_rx_checks, TESLA_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(tesla_rx_checks, TESLA_TX_MSGS);
}
ret = BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS);
return ret;
}

Expand Down
3 changes: 1 addition & 2 deletions python/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -217,9 +217,8 @@ class Panda:
FLAG_HYUNDAI_ESCC = 512
FLAG_HYUNDAI_NON_SCC = 1024

FLAG_TESLA_POWERTRAIN = 1
FLAG_TESLA_MODEL3_Y = 1
FLAG_TESLA_LONG_CONTROL = 2
FLAG_TESLA_RAVEN = 4

FLAG_VOLKSWAGEN_LONG_CONTROL = 1

Expand Down