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

Subaru LKAS Angle safety model #2020

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Subaru LKAS Angle safety model #2020

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
b25a894
LKAS model from jnewb
bravochar Aug 3, 2024
58205a1
Remove LKAS Angle from debug ifdef
bravochar Sep 13, 2024
0565c29
Add LKAS Angle safety tests
bravochar Sep 13, 2024
e0e61cc
Update cruise activated signal for LKAS Angle subaru
bravochar Sep 13, 2024
61c7076
Detect cruise on cam bus
bravochar Sep 13, 2024
a296efd
Add long tests?
bravochar Sep 13, 2024
1e385a2
Merge Gen2 and Angle together for LKAS Angle test
bravochar Sep 13, 2024
6057481
Fix rebase bugs
bravochar Nov 23, 2024
1c7ac6b
Fix misra erros
bravochar Nov 23, 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
57 changes: 51 additions & 6 deletions board/safety/safety_subaru.h
View file
Open in desktop
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,18 @@ 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) {
// LKAS Angle cars use different message
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 @@ -133,12 +145,25 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
}

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 +189,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 +243,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 +289,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 +303,21 @@ static safety_config subaru_init(uint16_t param) {
};

const uint16_t SUBARU_PARAM_GEN2 = 1;
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;

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
1 change: 1 addition & 0 deletions python/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,7 @@ class Panda:

FLAG_SUBARU_GEN2 = 1
FLAG_SUBARU_LONG = 2
FLAG_SUBARU_LKAS_ANGLE = 4

FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE = 1

Expand Down
4 changes: 4 additions & 0 deletions tests/safety/common.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -759,6 +759,10 @@ def test_tx_hook_on_wrong_safety_mode(self):
continue
if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruGen'):
continue
if attr.startswith('TestSubaruAngle') and current_test.startswith('TestSubaruGen'):
continue
if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruAngle'):
continue
if attr.startswith('TestSubaruPreglobal') and current_test.startswith('TestSubaruPreglobal'):
continue
if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
Expand Down
36 changes: 35 additions & 1 deletion tests/safety/test_subaru.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest):
ALT_MAIN_BUS = SUBARU_MAIN_BUS
ALT_CAM_BUS = SUBARU_CAM_BUS

DEG_TO_CAN = 100
DEG_TO_CAN = 100.

INACTIVE_GAS = 1818

Expand Down Expand Up @@ -167,6 +167,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 = Panda.FLAG_SUBARU_LKAS_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 All @@ -186,6 +213,13 @@ class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSaf
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)


class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS

FLAGS = Panda.FLAG_SUBARU_GEN2 | Panda.FLAG_SUBARU_LKAS_ANGLE
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)


class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = Panda.FLAG_SUBARU_LONG
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs(SUBARU_MAIN_BUS)
Expand Down
Loading