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.

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

Legacy and Outback port #717

Closed
wants to merge 35 commits into from
Closed
Changes from 1 commit
Commits
Show all changes
35 commits
Select commit Hold shift + click to select a range
7cb21f5
Legacy and Outback port
bugsy924 Jun 30, 2019
81f5e64
Subaru fixes
bugsy924 Jul 2, 2019
d30b473
Change outback tuning back to PID
bugsy924 Jul 13, 2019
dea794c
Merge branch 'devel' into Subaru-PR
bugsy924 Sep 10, 2019
0ac2a53
Update interface.py
bugsy924 Sep 11, 2019
94d11a3
moved units signal out of checks
bugsy924 Sep 12, 2019
cbf3eb6
match old subaru steer delta with impreza
bugsy924 Sep 12, 2019
2a57df3
Merge branch 'devel' into Subaru-PR
bugsy924 Nov 9, 2019
a7a53c9
Updated stuff
bugsy924 Nov 9, 2019
8efea56
Subaru fixes
bugsy924 Nov 9, 2019
2996962
Remove LEGACY LIMITED 2.5i 2015 fingerprint
bugsy924 Nov 9, 2019
07707fc
removed unused tire_stiffness_factor as requested by travis
bugsy924 Nov 9, 2019
4ebae40
moved global units from checks to signals
bugsy924 Nov 9, 2019
f7ca42e
Added missing impreza signals
bugsy924 Nov 9, 2019
4dcb5c7
Update interface.py
bugsy924 Nov 10, 2019
6792d55
Update carstate.py
bugsy924 Nov 10, 2019
6c36a86
Update interface.py
bugsy924 Nov 10, 2019
8f24296
Update carcontroller.py
bugsy924 Nov 10, 2019
2e0b503
add fingerprints from martinl's branch
bugsy924 Nov 11, 2019
b8eeb69
Update interface.py
bugsy924 Nov 11, 2019
4663c56
set torque to 0 when not enabled
bugsy924 Nov 12, 2019
c4f453c
Update carcontroller.py
bugsy924 Nov 12, 2019
f2ddc55
Update interface.py
bugsy924 Nov 12, 2019
6f20afd
Update interface.py
bugsy924 Nov 13, 2019
2457931
Update carcontroller.py
bugsy924 Nov 13, 2019
886bc8b
Merge remote-tracking branch 'upstream/devel' into Subaru-PR-Test
bugsy924 Nov 15, 2019
cb8506f
Prevent panda violation
bugsy924 Nov 17, 2019
b6d0362
Fix indentation
bugsy924 Nov 17, 2019
91f267f
Merge remote-tracking branch 'upstream/devel' into Subaru-PR
bugsy924 Nov 23, 2019
1e92f39
Merge branch 'Subaru-PR-Test' into Subaru-PR
bugsy924 Nov 23, 2019
7dc136e
Merge remote-tracking branch 'upstream/devel' into Subaru-PR-Test
bugsy924 Dec 17, 2019
08e3870
Update outback fingerprint
bugsy924 Dec 20, 2019
c3f1a2a
Merge remote-tracking branch 'upstream/devel' into Subaru-PR-Test
bugsy924 Jan 5, 2020
04bc62d
add subaru button message to panda tx
bugsy924 Jan 8, 2020
54818ad
Merge branch 'Subaru-PR-Test' into Subaru-PR
bugsy924 Jan 8, 2020
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -94,6 +94,8 @@ Supported Cars
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Subaru | Crosstrek 2018 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Outback 2015-17 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Subaru | Legacy 2015-17 | EyeSight | Yes | Stock | 0mph | 0mph | Custom<sup>4</sup>|
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Camry 2018 | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-18 | All | Yes | Stock | 0mph | 0mph | Toyota |
@@ -14,23 +14,22 @@ int subaru_desired_torque_last = 0;
uint32_t subaru_ts_last = 0;
struct sample_t subaru_torque_driver; // last few driver torques measured

static void subaru_init(int16_t param) {
}

static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;

if ((addr == 0x119) && (bus_number == 0)){
int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF);
if ((addr == 0x119 || addr == 0x371) && (bus_number == 0)){
int bit_shift = (addr == 0x119) ? 16 : 29;
int torque_driver_new = ((to_push->RDLR >> bit_shift) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&subaru_torque_driver, torque_driver_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x240) && (bus_number == 0)) {
int cruise_engaged = (to_push->RDHR >> 9) & 1;
if ((addr == 0x240 || addr == 0x144) && (bus_number == 0)) {
int bit_shift = (addr == 0x240) ? 9 : 17;
int cruise_engaged = (to_push->RDHR >> bit_shift) & 1;
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
@@ -44,8 +43,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr = to_send->RIR >> 21;

// steer cmd checks
if (addr == 0x122) {
int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF);
if (addr == 0x122 || addr == 0x164) {
int bit_shift = (addr == 0x122) ? 16 : 8;
int desired_torque = ((to_send->RDLR >> bit_shift) & 0x1FFF);
int violation = 0;
uint32_t ts = TIM2->CNT;
desired_torque = to_signed(desired_torque, 13);
@@ -97,43 +97,27 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {

// shifts bits 29 > 11
int32_t addr = to_fwd->RIR >> 21;

// forward CAN 0 > 1
int bus_fwd = -1;
if (bus_num == 0) {

return 2; // ES CAN
}
// forward CAN 1 > 0, except ES_LKAS
else if (bus_num == 2) {

// outback 2015
if (addr == 0x164) {
return -1;
}
// global platform
if (addr == 0x122) {
return -1;
}
// ES Distance
if (addr == 545) {
return -1;
bus_fwd = 2; // Camera CAN
} else if (bus_num == 2) {
// 356 is LKAS for outback 2015
// 356 is LKAS for Global Platform
// 545 is ES_Distance
// 802 is ES_LKAS
int32_t addr = to_fwd->RIR >> 21;
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
if (!block_msg) {
bus_fwd = 0; // Main CAN
}
// ES LKAS
if (addr == 802) {
return -1;
}

return 0; // Main CAN
}

// fallback to do not forward
return -1;
return bus_fwd;
}

const safety_hooks subaru_hooks = {
.init = subaru_init,
.init = nooutput_init,
.rx = subaru_rx_hook,
.tx = subaru_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
@@ -16,7 +16,11 @@ def __init__(self, car_fingerprint):
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc

if car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
self.STEER_DRIVER_ALLOWANCE = 300 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 1 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc
self.STEER_DELTA_DOWN = 60 # torque decrease per refresh

This comment has been minimized.

Copy link
@rbiasini

rbiasini Sep 12, 2019

mmm... not using the impreza params will be quite difficult to manage in panda safety I think. At the moment, how does it work in your fork?

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Nov 1, 2019

Author

The message length is 1 bit longer on pre-global. I don't currently know how to make it work, I think I cheated with the bit shifting for the legacy and outback to trim the lsb to match impreza length to not enable a violation. It would have been great if subaru kept the same CAN bus IDs but it's almost a separate make port



class CarController(object):
@@ -28,6 +32,7 @@ def __init__(self, car_fingerprint):
self.car_fingerprint = car_fingerprint
self.es_distance_cnt = -1
self.es_lkas_cnt = -1
self.counter = 0

# Setup detection helper. Routes commands to
# an appropriate CAN bus number.
@@ -46,30 +51,39 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, le
### STEER ###

if (frame % P.STEER_STEP) == 0:

final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))

# limits due to driver torque

apply_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)

lkas_enabled = enabled and not CS.steer_not_allowed
if not enabled:
apply_steer = 0.

if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):

if not lkas_enabled:
apply_steer = 0
# add noise to prevent lkas fault from constant torque value for over 1s
if enabled and apply_steer == self.apply_steer_last:
self.counter =+ 1
if self.counter == 50:
apply_steer = round(int(apply_steer * 0.99))
else:
self.counter = 0

can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))

self.apply_steer_last = apply_steer

if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.car_fingerprint == CAR.IMPREZA:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]

if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]

if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY) and pcm_cancel_cmd:
can_sends.append(subarucan.create_door_control(self.packer))

return can_sends
@@ -2,14 +2,15 @@
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
from selfdrive.can.parser import CANParser
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
from selfdrive.car.subaru.values import CAR, DBC, STEER_THRESHOLD

def get_powertrain_can_parser(CP):
# this function generates lists for signal, messages and initial values
signals = [
# sig_name, sig_address, default
("Steer_Torque_Sensor", "Steering_Torque", 0),
("Steering_Angle", "Steering_Torque", 0),
("Steer_Torque_Output", "Steering_Torque", 0),
("Cruise_On", "CruiseControl", 0),
("Cruise_Activated", "CruiseControl", 0),
("Brake_Pedal", "Brake_Pedal", 0),
@@ -25,60 +26,69 @@ def get_powertrain_can_parser(CP):
("DOOR_OPEN_FL", "BodyInfo", 1),
("DOOR_OPEN_RR", "BodyInfo", 1),
("DOOR_OPEN_RL", "BodyInfo", 1),
("Units", "Dash_State", 1),
]

checks = [
# sig_address, frequency
("Dashlights", 10),
("CruiseControl", 20),
("Wheel_Speeds", 50),
("Steering_Torque", 50),
("BodyInfo", 10),
]

return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
if CP.carFingerprint == CAR.IMPREZA:
checks += [
("BodyInfo", 10),
("CruiseControl", 20),
("Units", "Dash_State", 1),
]

else:
signals += [
("LKA_Lockout", "Steering_Torque", 0),
]
checks += [
("CruiseControl", 50),
]

return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)

def get_camera_can_parser(CP):
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),

("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Main", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),

("Checksum", "ES_LKAS_State", 0),
("Counter", "ES_LKAS_State", 0),
("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
("Empty_Box", "ES_LKAS_State", 0),
("Signal1", "ES_LKAS_State", 0),
("LKAS_ACTIVE", "ES_LKAS_State", 0),
("Signal2", "ES_LKAS_State", 0),
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
("LKAS_ENABLE_3", "ES_LKAS_State", 0),
("Signal3", "ES_LKAS_State", 0),
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
("Signal4", "ES_LKAS_State", 0),
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
("Signal6", "ES_LKAS_State", 0),
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
("Signal7", "ES_LKAS_State", 0),
("FCW_Cont_Beep", "ES_LKAS_State", 0),
("FCW_Repeated_Beep", "ES_LKAS_State", 0),
("Throttle_Management_Activated", "ES_LKAS_State", 0),
("Traffic_light_Ahead", "ES_LKAS_State", 0),
("Right_Depart", "ES_LKAS_State", 0),
("Signal5", "ES_LKAS_State", 0),

]

checks = [
("ES_DashStatus", 10),
]

if CP.carFingerprint == CAR.IMPREZA:
signals += [
("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Main", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),

("Checksum", "ES_LKAS_State", 0),
("Counter", "ES_LKAS_State", 0),
("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
("Empty_Box", "ES_LKAS_State", 0),
("Signal1", "ES_LKAS_State", 0),
("LKAS_ACTIVE", "ES_LKAS_State", 0),
("Signal2", "ES_LKAS_State", 0),
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
("LKAS_ENABLE_3", "ES_LKAS_State", 0),
("Signal3", "ES_LKAS_State", 0),
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
("Signal4", "ES_LKAS_State", 0),
("FCW_Cont_Beep", "ES_LKAS_State", 0),
("FCW_Repeated_Beep", "ES_LKAS_State", 0),
("Throttle_Management_Activated", "ES_LKAS_State", 0),
("Traffic_light_Ahead", "ES_LKAS_State", 0),
("Right_Depart", "ES_LKAS_State", 0),
("Signal5", "ES_LKAS_State", 0),
]

return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)


@@ -121,12 +131,9 @@ def update(self, cp, cp_cam):
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS

self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
# 1 = imperial, 6 = metric
if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH

v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]

@@ -143,6 +150,7 @@ def update(self, cp, cp_cam):
self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1
self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
self.steer_torque_motor = cp.vl["Steering_Torque"]['Steer_Torque_Output']

This comment has been minimized.

Copy link
@rbiasini

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Jul 1, 2019

Author

Unused now, yes, I can remove it
Previously used when using the same steer limit as Toyota

self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
self.main_on = cp.vl["CruiseControl"]['Cruise_On']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
@@ -152,5 +160,13 @@ def update(self, cp, cp_cam):
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
cp.vl["BodyInfo"]['DOOR_OPEN_FL']])

self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
if self.car_fingerprint == CAR.IMPREZA:
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.MPH_TO_KPH
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
# 1 = imperial, 6 = metric
if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH
else:
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"]
self.steer_not_allowed = cp.vl["Steering_Torque"]["LKA_Lockout"]

This comment has been minimized.

Copy link
@rbiasini

rbiasini Jul 1, 2019

this breaks the Impreza. All the cars shall have the same attributes.

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Jul 1, 2019

Author

Which lines break the Impreza, 163-169? Should be the same. I'll look into it more later. The Outback and Legacy are on a very different platform

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Jul 1, 2019

Author

I'll change that else to outback and legacy

This comment has been minimized.

Copy link
@rbiasini

rbiasini Jul 1, 2019

steer_not_allowed is used in interface. so needs to be defined for all cars

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Jul 1, 2019

Author

Ah I see now, I'll fix these all tonight. Thanks

This comment has been minimized.

Copy link
@bugsy924

bugsy924 Jul 3, 2019

Author

I made the changes
Would you like me to redo this on 0.6?

@@ -67,9 +67,36 @@ def get_params(candidate, fingerprint, vin=""):
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]

if candidate in [CAR.OUTBACK]:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
tire_stiffness_factor = 1
ret.steerActuatorDelay = 0.3
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.3
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]

if candidate in [CAR.LEGACY]:
ret.mass = 1568 + std_cargo
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 12.5 #14.5 stock
tire_stiffness_factor = 1.0
ret.steerActuatorDelay = 0.15
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1, 0.2], [0.01, 0.02]]
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]

ret.steerControlType = car.CarParams.SteerControlType.torque
ret.steerRatioRear = 0.
# testing tuning

# No long control in subaru
ret.gasMaxBP = [0.]
@@ -83,8 +110,6 @@ def get_params(candidate, fingerprint, vin=""):
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]

# end from gm

# hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923./2.205 + std_cargo
@@ -182,6 +207,9 @@ def update(self, c):
else:
self.can_invalid_count = 0

if self.CS.steer_not_allowed:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))

if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))

ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.