8000 Subaru: 2023 Ascent & LKAS Angle by bravochar · Pull Request #2217 · commaai/opendbc · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Subaru: 2023 Ascent & LKAS Angle #2217

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Subaru: 2023 Ascent & LKAS Angle #2217

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
58 changes: 38 additions & 20 deletions opendbc/car/subaru/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
import numpy as np
from opendbc.can.packer import CANPacker
from opendbc.car import Bus, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
from opendbc.car import Bus, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, apply_std_steer_angle_limits
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.subaru import subarucan
from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
Expand All @@ -15,6 +15,7 @@ class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.apply_torque_last = 0
self.apply_steer_last = 0

self.cruise_button_prev = 0
self.steer_rate_counter = 0
Expand All @@ -31,30 +32,44 @@ def update(self, CC, CS, now_nanos):

# *** steering ***
if (self.frame % self.p.STEER_STEP) == 0:
apply_torque = int(round(actuators.torque * self.p.STEER_MAX))
apply_steer = 0
apply_torque = 0
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw,
CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS)

# limits due to driver torque
if not CC.latActive:
apply_steer = CS.out.steeringAngleDeg

new_torque = int(round(apply_torque))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive))
self.apply_steer_last = apply_steer

if not CC.latActive:
apply_torque = 0

if self.CP.flags & SubaruFlags.PREGLOBAL:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
# torque-based steering
else:
apply_stee 10000 r_req = CC.latActive
apply_torque = int(round(actuators.torque * self.p.STEER_MAX))

# limits due to driver torque

if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
# Steering rate fault prevention
self.steer_rate_counter, apply_steer_req = \
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
new_torque = int(round(apply_torque))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)

can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))
if not CC.latActive:
apply_torque = 0

if self.CP.flags & SubaruFlags.PREGLOBAL:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
else:
apply_steer_req = CC.latActive

self.apply_torque_last = apply_torque
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
# Steering rate fault prevention
self.steer_rate_counter, apply_steer_req = \
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)

can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))

self.apply_torque_last = apply_torque

# *** longitudinal ***

Expand Down Expand Up @@ -136,8 +151,11 @@ def update(self, CC, CS, now_nanos):
can_sends.append(subarucan.create_es_static_2(self.packer))

new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
new_actuators.steeringAngleDeg = self.apply_steer_last
else:
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last

self.frame += 1
return new_actuators, can_sends
12 changes: 9 additions & 3 deletions opendbc/car/subaru/carstate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,12 +78,18 @@ def update(self, can_parsers) -> structs.CarState:
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold

cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
if self.CP.flags & SubaruFlags.HYBRID:
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0

else:
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
if self.CP.flags & SubaruFlags.HYBRID:
ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
else:
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0

ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS

if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \
Expand Down
18 changes: 11 additions & 7 deletions opendbc/car/subaru/fingerprints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,13 +42,17 @@
],
(Ecu.fwdCamera, 0x787, None): [
b'\x05!\x08\x1dK\x05!\x08\x01/',
],
(Ecu.engine, 0x7a2, None): [
b'\xe5,\xa0P\x07',
],
(Ecu.transmission, 0x7a3, None): [
b'\x04\xfe\xf3\x00\x00',
],
b'\x05!\x08\x1dK\x00\x00\x00\x00\x00',
],
# XXX: No longer showing up?!
#(Ecu.engine, 0x7a2, None): [
# b'\xe5,\xa0P\x07',
# b'\xe5,\xa0p\x07',
#],
#(Ecu.transmission, 0x7a3, None): [
# b'\x04\xfe\xf3\x00\x00',
# b'\x04\xfe\xf6\x00\x00',
#],
},
CAR.SUBARU_LEGACY: {
(Ecu.abs, 0x7b0, None): [
Expand Down
14 changes: 13 additions & 1 deletion opendbc/car/subaru/interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)]
if ret.flags & SubaruFlags.GLOBAL_GEN2:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value
if ret.flags & SubaruFlags.LKAS_ANGLE:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value

ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
Expand All @@ -42,13 +44,23 @@ def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alp
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)

if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
if candidate == CAR.SUBARU_ASCENT:
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00003
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]

elif candidate == CAR.SUBARU_ASCENT_2023:
ret.dashcamOnly = False
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00003
ret.lateralTuning.pid.kpBP = [0., 20.]
ret.lateralTuning.pid.kiBP = [0., 20.]
ret.lateralTuning.pid.kpV = [0.0025, 0.1]
ret.lateralTuning.pid.kiV = [0.00025, 0.01]

elif candidate == CAR.SUBARU_IMPREZA:
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
ret.lateralTuning.init('pid')
Expand Down
11 changes: 9 additions & 2 deletions opendbc/car/subaru/values.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag

from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car import AngleSteeringLimits, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Expand All @@ -10,6 +10,12 @@


class CarControllerParams:
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
100,
([0., 15., 15.], [5., .8, .8]),
([0., 15., 15.], [5., .4, 0.4]),
)

def __init__(self, CP):
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
Expand Down Expand Up @@ -57,6 +63,7 @@ class SubaruSafetyFlags(IntFlag):
GEN2 = 1
LONG = 2
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
LKAS_ANGLE = 8


class SubaruFlags(IntFlag):
Expand Down Expand Up @@ -276,7 +283,7 @@ class CAR(Platforms):
# We don't get the EPS from non-OBD queries on GEN2 cars. Note that we still attempt to match when it exists
non_essential_ecus={
Ecu.eps: list(CAR.with_flags(SubaruFlags.GLOBAL_GEN2)),
}
},
)

DBC = CAR.create_dbc_map()
61 changes: 56 additions & 5 deletions opendbc/safety/modes/subaru.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,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,8 +71,16 @@
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \

#define SUBARU_LKAS_ANGLE_RX_CHECKS(alt_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \

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 @@ -93,7 +102,7 @@ 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;

int addr = GET_ADDR(to_push);
if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
Expand All @@ -109,9 +118,17 @@ 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 Down Expand Up @@ -139,6 +156,19 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);

const AngleSteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
.max_angle = 500*100,
.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 +194,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 @@ -229,6 +267,11 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};

static const CanMsg subaru_lkas_angle_tx_msgs[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};

static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
Expand All @@ -237,17 +280,25 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
};

static RxCheck subaru_lkas_angle_rx_checks[] = {
SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS)
};

const uint16_t SUBARU_PARAM_GEN2 = 1;
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_lkas_angle_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
7 changes: 6 additions & 1 deletion opendbc/safety/tests/common.py
View file
Open in desktop
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LKAS_ANGLE is a signed, 17-bit integer, so angle values larger than ~655 degrees will over/underflow and this test will erroneously fail.

Original file line number Diff line number Diff line change
Expand Up @@ -694,7 +694,8 @@ def test_steering_angle_measurements(self):
def test_angle_cmd_when_enabled(self):
# when controls are allowed, angle cmd rate limit is enforced
speeds = [0., 1., 5., 10., 15., 50.]
angles = np.concatenate((np.arange(-self.STEER_ANGLE_MAX * 2, self.STEER_ANGLE_MAX * 2, 5), [0]))
angle_max_abs = self.STEER_ANGLE_MAX + 10
angles = np.concatenate((np.arange(-angle_max_abs, angle_max_abs, 5), [0]))
for a in angles:
for s in speeds:
max_delta_up = np.interp(s, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP)
Expand Down Expand Up @@ -851,6 +852,10 @@ def test_tx_hook_on_wrong_safety_mode(self):
if attr == 'TestHyundaiCanfdLKASteeringLongEV' and current_test.startswith('TestToyota'):
tx 601E = list(filter(lambda m: m[0] not in [0x160, ], tx))

# Rivian message overlaps with Subaru LKAS Angle ES_DashStatus message
if attr.startswith('TestRivian') and current_test.startswith('TestSubaruGen2Angle'):
tx = list(filter(lambda m: m[0] not in [0x321, ], tx))

# Volkswagen MQB longitudinal actuating message overlaps with the Subaru lateral actuating message
if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestSubaru'):
tx = list(filter(lambda m: m[0] not in [0x122, ], tx))
Expand Down
Loading
0