Skip to content

Commit

Permalink
add outback safety
Browse files Browse the repository at this point in the history
  • Loading branch information
@jnewb1
jnewb1 committed Jul 22, 2023
1 parent 3b96f0d commit 49961c7
Show file tree
Hide file tree
Showing 3 changed files with 63 additions and 32 deletions.
37 changes: 26 additions & 11 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,11 @@ const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
};
#define SUBARU_LKAS_ANGLE_TX_MSGS_LEN (sizeof(SUBARU_LKAS_ANGLE_TX_MSGS) / sizeof(SUBARU_LKAS_ANGLE_TX_MSGS[0]))

const CanMsg SUBARU_LKAS_ANGLE_GEN2_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
};
#define SUBARU_LKAS_ANGLE_GEN2_TX_MSGS_LEN (sizeof(SUBARU_LKAS_ANGLE_GEN2_TX_MSGS) / sizeof(SUBARU_LKAS_ANGLE_GEN2_TX_MSGS[0]))

AddrCheckStruct subaru_addr_checks[] = {
SUBARU_COMMON_ADDR_CHECKS(SUBARU_MAIN_BUS)
SUBARU_GEN12_ADDR_CHECKS(SUBARU_MAIN_BUS)
Expand Down Expand Up @@ -135,7 +140,8 @@ static int subaru_rx_hook(CANPacket_t *to_push) {

if (valid) {
const int bus = GET_BUS(to_push);
const int alt_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const int alt_cam_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS;
const int stock_ecu = lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;

int addr = GET_ADDR(to_push);
Expand All @@ -150,18 +156,18 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_bus) && !es_status) {
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus) && !es_status) {
bool cruise_engaged = GET_BIT(to_push, 41U) != 0U;
pcm_cruise_check(cruise_engaged);
}

if ((addr == MSG_SUBARU_ES_Status) && (bus == SUBARU_CAM_BUS) && es_status) {
if ((addr == MSG_SUBARU_ES_Status) && (bus == alt_cam_bus) && es_status) {
bool cruise_engaged = GET_BIT(to_push, 29U) != 0U;
pcm_cruise_check(cruise_engaged);
}

// update vehicle moving with any non-zero wheel speed
if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_bus)) {
if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_main_bus)) {
uint32_t fr = (GET_BYTES(to_push, 1, 3) >> 4) & 0x1FFFU;
uint32_t rr = (GET_BYTES(to_push, 3, 3) >> 1) & 0x1FFFU;
uint32_t rl = (GET_BYTES(to_push, 4, 3) >> 6) & 0x1FFFU;
Expand All @@ -173,7 +179,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
}

if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_bus)) {
if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {
brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1U);
}

Expand All @@ -191,12 +197,21 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);

if (subaru_gen2) {
tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
} else if (lkas_angle) {
tx = msg_allowed(to_send, SUBARU_LKAS_ANGLE_TX_MSGS, SUBARU_LKAS_ANGLE_TX_MSGS_LEN);
} else {
tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
if(lkas_angle) {
if(subaru_gen2){
tx = msg_allowed(to_send, SUBARU_LKAS_ANGLE_GEN2_TX_MSGS, SUBARU_LKAS_ANGLE_GEN2_TX_MSGS_LEN);
}
else{
tx = msg_allowed(to_send, SUBARU_LKAS_ANGLE_TX_MSGS, SUBARU_LKAS_ANGLE_TX_MSGS_LEN);
}
}
else{
if(subaru_gen2){
tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
}
else{
tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
}
}

// steer cmd checks
Expand Down
4 changes: 2 additions & 2 deletions tests/safety/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -932,9 +932,9 @@ def test_tx_hook_on_wrong_safety_mode(self):
if attr.startswith('TestHonda'):
# exceptions for common msgs across different hondas
tx = list(filter(lambda m: m[0] not in [0x1FA, 0x30C, 0x33D], tx))
if attr in ['TestSubaruGen1Safety', 'TestSubaruGen2Safety', 'TestSubaruForester2022Safety']:
if attr in ['TestSubaruGen1Safety', 'TestSubaruGen2Safety', 'TestSubaruForester2022Safety', 'TestSubaruOutback2023Safety']:
# exceptions for common msgs across different subarus
tx = list(filter(lambda m: m[0] not in [0x122, 0x221, 0x321, 0x322, 0x323], tx))
tx = list(filter(lambda m: m[0] not in [0x122, 0x124, 0x221, 0x321, 0x322, 0x323], tx))
all_tx.append(list([m[0], m[1], attr] for m in tx))

# make sure we got all the msgs
Expand Down
54 changes: 35 additions & 19 deletions tests/safety/test_subaru.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ class TestSubaruSafetyBase(common.PandaSafetyTest):
DRIVER_TORQUE_ALLOWANCE = 60
DRIVER_TORQUE_FACTOR = 50

ALT_BUS = SUBARU_MAIN_BUS
ALT_MAIN_BUS = SUBARU_MAIN_BUS
ALT_CAM_BUS = SUBARU_CAM_BUS

VEHICLE_SPEED_PRECISION = 3

Expand All @@ -74,19 +75,19 @@ def _torque_driver_msg(self, torque):

def _speed_msg(self, speed):
values = {s: speed for s in ["FR", "FL", "RR", "RL"]}
return self.packer.make_can_msg_panda("Wheel_Speeds", self.ALT_BUS, values)
return self.packer.make_can_msg_panda("Wheel_Speeds", self.ALT_MAIN_BUS, values)

def _user_brake_msg(self, brake):
values = {"Brake": brake}
return self.packer.make_can_msg_panda("Brake_Status", self.ALT_BUS, values)
return self.packer.make_can_msg_panda("Brake_Status", self.ALT_MAIN_BUS, values)

def _user_gas_msg(self, gas):
values = {"Throttle_Pedal": gas}
return self.packer.make_can_msg_panda("Throttle", 0, values)

def _pcm_status_msg(self, enable):
values = {"Cruise_Activated": enable}
return self.packer.make_can_msg_panda("CruiseControl", self.ALT_BUS, values)
return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values)


class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeringSafetyTest):
Expand All @@ -95,18 +96,19 @@ def _torque_cmd_msg(self, torque, steer_req=1):
return self.packer.make_can_msg_panda("ES_LKAS", 0, values)


class TestSubaruGen2SafetyBase(TestSubaruTorqueSafetyBase):
ALT_BUS = SUBARU_ALT_BUS

MAX_RATE_UP = 40
MAX_RATE_DOWN = 40
MAX_TORQUE = 1000

class TestSubaruGen1Safety(TestSubaruTorqueSafetyBase):
FLAGS = 0
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)

class TestSubaruGen2Safety(TestSubaruGen2SafetyBase):
class TestSubaruGen2Safety(TestSubaruTorqueSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
ALT_CAM_BUS = SUBARU_ALT_BUS

MAX_RATE_UP = 40
MAX_RATE_DOWN = 40
MAX_TORQUE = 1000

FLAGS = Panda.FLAG_SUBARU_GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)

Expand All @@ -120,6 +122,12 @@ class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafety

FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE | Panda.FLAG_SUBARU_ES_STATUS

ANGLE_RATE_BP = [0]
ANGLE_RATE_UP = [1]
ANGLE_RATE_DOWN = [1]

ANGLE_PRECISION = 2

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)
Expand All @@ -129,16 +137,24 @@ def _angle_meas_msg(self, angle):
return self.packer.make_can_msg_panda("Steering_Torque", 0, values)


class TestSubaruForester2022Safety(TestSubaruAngleSafetyBase):
ANGLE_RATE_BP = [0]
ANGLE_RATE_UP = [1]
ANGLE_RATE_DOWN = [1]

ANGLE_PRECISION = 2

class TestESStatusBase(TestSubaruAngleSafetyBase):
def _pcm_status_msg(self, enable):
values = {"Cruise_Activated": enable}
return self.packer.make_can_msg_panda("ES_Status", 2, values)
return self.packer.make_can_msg_panda("ES_Status", self.ALT_CAM_BUS, values)


class TestSubaruForester2022Safety(TestESStatusBase):
pass


class TestSubaruOutback2023Safety(TestESStatusBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
ALT_CAM_BUS = SUBARU_ALT_BUS

TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)

FLAGS = Panda.FLAG_SUBARU_GEN2 | Panda.FLAG_SUBARU_LKAS_ANGLE | Panda.FLAG_SUBARU_ES_STATUS


if __name__ == "__main__":
unittest.main()

0 comments on commit 49961c7

Please sign in to comment.