Skip to content

Commit

Permalink
Add panda safety code
Browse files Browse the repository at this point in the history
  • Loading branch information
@fredyshox
fredyshox committed Feb 22, 2025
1 parent 3d73100 commit b5774cf
Show file tree
Hide file tree
Showing 3 changed files with 77 additions and 8 deletions.
1 change: 1 addition & 0 deletions opendbc/car/subaru/values.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ class SubaruSafetyFlags(IntFlag):
GEN2 = 1
LONG = 2
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
ANGLE = 8


class SubaruFlags(IntFlag):
Expand Down
57 changes: 49 additions & 8 deletions opendbc/safety/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#define MSG_SUBARU_Wheel_Speeds 0x13a

#define MSG_SUBARU_ES_LKAS 0x122
#define MSG_SUBARU_ES_LKAS_ANGLE 0x124
#define MSG_SUBARU_ES_Brake 0x220
#define MSG_SUBARU_ES_Distance 0x221
#define MSG_SUBARU_ES_Status 0x222
Expand Down Expand Up @@ -70,6 +71,7 @@

static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
static bool subaru_lkas_angle = false;

static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
return (uint8_t)GET_BYTE(to_push, 0);
Expand All @@ -91,7 +93,8 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) {

static void subaru_rx_hook(const CANPacket_t *to_push) {
const int bus = GET_BUS(to_push);
const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const int alt_main_bus = (subaru_gen2 || subaru_lkas_angle) ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;

int addr = GET_ADDR(to_push);
if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
Expand All @@ -107,9 +110,16 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
bool cruise_engaged = GET_BIT(to_push, 41U);
pcm_cruise_check(cruise_engaged);
if (subaru_lkas_angle) {
if ((addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
bool cruise_engaged = GET_BIT(to_push, 36U);
pcm_cruise_check(cruise_engaged);
}
} else {
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
bool cruise_engaged = GET_BIT(to_push, 41U);
pcm_cruise_check(cruise_engaged);
}
}

// update vehicle moving with any non-zero wheel speed
Expand All @@ -132,13 +142,25 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
gas_pressed = GET_BYTE(to_push, 4) != 0U;
}

generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS));
generic_rx_checks((addr == stock_lkas_msg) && (bus == SUBARU_MAIN_BUS));
}

static bool subaru_tx_hook(const CANPacket_t *to_send) {
const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);

const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
.angle_deg_to_can = 100.,
.angle_rate_up_lookup = {
{0., 15., 15.},
{5., .8, .8}
},
.angle_rate_down_lookup = {
{0., 15., 15.},
{5., .4, .4}
},
};

const LongitudinalLimits SUBARU_LONG_LIMITS = {
.min_gas = 808, // appears to be engine braking
.max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle
Expand All @@ -164,6 +186,14 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
}

if (addr == MSG_SUBARU_ES_LKAS_ANGLE) {
int desired_angle = GET_BYTES(to_send, 5, 3) & 0x1FFFFU;
desired_angle = -1 * to_signed(desired_angle, 17);
bool lkas_request = GET_BIT(to_send, 12U);

violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS);
}

// check es_brake brake_pressure limits
if (addr == MSG_SUBARU_ES_Brake) {
int es_brake_pressure = GET_BYTES(to_send, 2, 2);
Expand Down Expand Up @@ -210,13 +240,15 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
static int subaru_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;

const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;

if (bus_num == SUBARU_MAIN_BUS) {
bus_fwd = SUBARU_CAM_BUS; // to the eyesight camera
}

if (bus_num == SUBARU_CAM_BUS) {
// Global platform
bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) ||
bool block_lkas = ((addr == stock_lkas_msg) ||
(addr == MSG_SUBARU_ES_DashStatus) ||
(addr == MSG_SUBARU_ES_LKAS_State) ||
(addr == MSG_SUBARU_ES_Infotainment));
Expand Down Expand Up @@ -254,6 +286,11 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};

static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
};

static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
Expand All @@ -263,16 +300,20 @@ static safety_config subaru_init(uint16_t param) {
};

const uint16_t SUBARU_PARAM_GEN2 = 1;
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 8;

subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);

#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
#endif

safety_config ret;
if (subaru_gen2) {
if (subaru_lkas_angle) {
ret = BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
} else if (subaru_gen2) {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
} else {
Expand Down
27 changes: 27 additions & 0 deletions opendbc/safety/tests/test_subaru.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,33 @@ def _torque_cmd_msg(self, torque, steer_req=1):
return self.packer.make_can_msg_panda("ES_LKAS", SUBARU_MAIN_BUS, values)


class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
ALT_MAIN_BUS = SUBARU_ALT_BUS

TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_LKAS,)}
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE)

FLAGS = SubaruSafetyFlags.ANGLE

ANGLE_RATE_BP = [0, 15]
ANGLE_RATE_UP = [5, 0.15]
ANGLE_RATE_DOWN = [5, 0.4]

def _angle_cmd_msg(self, angle, enabled=1):
values = {"LKAS_Output": angle, "LKAS_Request": enabled}
return self.packer.make_can_msg_panda("ES_LKAS_ANGLE", 0, values)

def _angle_meas_msg(self, angle):
values = {"Steering_Angle": angle}
return self.packer.make_can_msg_panda("Steering_Torque", 0, values)

# need to use ES_DashStatus Message
def _pcm_status_msg(self, enable):
values = {"Cruise_Activated": enable}
return self.packer.make_can_msg_panda("ES_DashStatus", self.ALT_CAM_BUS, values)


class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = 0
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
Expand Down

0 comments on commit b5774cf

Please sign in to comment.