sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Subaru: Crosstrek Hybrid dashcam mode (#25378)

Browse Source 
* Subaru: Crosstrek Hybrid support

* hybrid cars in dashcam only

* revert that

* just dashcam

* tah should be abs

* remove unused import

* use the old dbc for now

* hybrid car exceptions in carstate

* need dashstatus until we get a proper cruiseactivated bit

* missing CP

* merge conflicts

* lets be consistent about extend

* cleanup

* fix and comments

* preglobal fix

* and the rest

* and this

* added hybrid to release

---------

Co-authored-by: Justin Newberry <justin@comma.ai>
pull/28400/head
Adeeb Shihadeh 2 years ago committed by GitHub
parent ddd849a882
commit 973d90be77
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 101 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      release/files_common
  2. 2
      selfdrive/car/subaru/carcontroller.py
  3. 70
      selfdrive/car/subaru/carstate.py
  4. 17
      selfdrive/car/subaru/interface.py
  5. 23
      selfdrive/car/subaru/values.py
  6. 1
      selfdrive/car/tests/routes.py
  7. 1
      selfdrive/car/torque_data/substitute.yaml

1
release/files_common
Unescape View File

@ -569,6 +569,7 @@ opendbc/nissan_x_trail_2017_generated.dbc
opendbc/nissan_leaf_2018_generated.dbc
opendbc/subaru_global_2017_generated.dbc
opendbc/subaru_global_2020_hybrid_generated.dbc
opendbc/subaru_outback_2015_generated.dbc
opendbc/subaru_outback_2019_generated.dbc
opendbc/subaru_forester_2017_generated.dbc

2
selfdrive/car/subaru/carcontroller.py
Unescape View File

@ -3,6 +3,7 @@ from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
from openpilot.selfdrive.car.subaru import subarucan
from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags
from selfdrive.car.subaru.values import HYBRID_CARS
class CarController:
@ -100,6 +101,7 @@ class CarController:
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
if pcm_cancel_cmd:
if self.CP.carFingerprint not in HYBRID_CARS:
bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))

70
selfdrive/car/subaru/carstate.py
Unescape View File

@ -6,6 +6,7 @@ from openpilot.selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags
from openpilot.selfdrive.car import CanSignalRateCalculator
from selfdrive.car.subaru.values import HYBRID_CARS
class CarState(CarStateBase):
@ -19,7 +20,9 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255.
throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"]
ret.gas = throttle_msg["Throttle_Pedal"] / 255.
ret.gasPressed = ret.gas > 1e-5
if self.car_fingerprint in PREGLOBAL_CARS:
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2
@ -46,7 +49,8 @@ class CarState(CarStateBase):
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
can_gear = int(cp.vl["Transmission"]["Gear"])
cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
@ -62,6 +66,10 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
if self.car_fingerprint in HYBRID_CARS:
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
@ -77,7 +85,7 @@ class CarState(CarStateBase):
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam
if self.car_fingerprint in PREGLOBAL_CARS:
self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
@ -87,17 +95,22 @@ class CarState(CarStateBase):
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
(cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
# 8 is known AEB, there are a few other values related to AEB we ignore
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
if self.car_fingerprint not in HYBRID_CARS: # Hybrid cars don't have es_distance, need a replacement
# 8 is known AEB, there are a few other values related to AEB we ignore
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
self.brake_status_msg = copy.copy(cp_brakes.vl["Brake_Status"])
if self.car_fingerprint not in HYBRID_CARS:
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
@ -106,22 +119,27 @@ class CarState(CarStateBase):
return ret
@staticmethod
def get_common_global_body_messages():
def get_common_global_body_messages(CP):
messages = [
("CruiseControl", 20),
("Wheel_Speeds", 50),
("Brake_Status", 50),
]
if CP.carFingerprint not in HYBRID_CARS:
messages.append(("CruiseControl", 20))
return messages
@staticmethod
def get_common_global_es_messages():
def get_common_global_es_messages(CP):
messages = [
("ES_Brake", 20),
]
if CP.carFingerprint not in HYBRID_CARS:
messages += [
("ES_Distance", 20),
("ES_Status", 20),
("ES_Brake", 20),
("ES_Status", 20)
]
return messages
@ -130,12 +148,16 @@ class CarState(CarStateBase):
def get_can_parser(CP):
messages = [
# sig_address, frequency
("Throttle", 100),
("Dashlights", 10),
("Brake_Pedal", 50),
("Transmission", 100),
("Steering_Torque", 50),
("BodyInfo", 1),
("Brake_Pedal", 50),
]
if CP.carFingerprint not in HYBRID_CARS:
messages += [
("Throttle", 100),
("Transmission", 100)
]
if CP.enableBsm:
@ -143,7 +165,7 @@ class CarState(CarStateBase):
if CP.carFingerprint not in PREGLOBAL_CARS:
if CP.carFingerprint not in GLOBAL_GEN2:
messages += CarState.get_common_global_body_messages()
messages += CarState.get_common_global_body_messages(CP)
messages += [
("Dashlights", 10),
@ -184,7 +206,7 @@ class CarState(CarStateBase):
]
if CP.carFingerprint not in GLOBAL_GEN2:
messages += CarState.get_common_global_es_messages()
messages += CarState.get_common_global_es_messages(CP)
if CP.flags & SubaruFlags.SEND_INFOTAINMENT:
messages.append(("ES_Infotainment", 10))
@ -193,9 +215,17 @@ class CarState(CarStateBase):
@staticmethod
def get_body_can_parser(CP):
messages = []
if CP.carFingerprint in GLOBAL_GEN2:
messages = CarState.get_common_global_body_messages()
messages += CarState.get_common_global_es_messages()
messages += CarState.get_common_global_body_messages(CP)
messages += CarState.get_common_global_es_messages(CP)
if CP.carFingerprint in HYBRID_CARS:
messages += [
("Throttle_Hybrid", 40),
("Transmission", 100)
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)
return None

17
selfdrive/car/subaru/interface.py
Unescape View File

@ -3,7 +3,7 @@ from cereal import car
from panda import Panda
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags
class CarInterface(CarInterfaceBase):
@ -12,7 +12,11 @@ class CarInterface(CarInterfaceBase):
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "subaru"
ret.radarUnavailable = True
ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE)
# for HYBRID CARS to be upstreamed, we need:
# - replacement for ES_Distance so we can cancel the cruise control
# - to find the Cruise_Activated bit from the car
# - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
ret.autoResumeSng = False
# Detect infotainment message sent from the camera
@ -68,6 +72,13 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
elif candidate == CAR.CROSSTREK_HYBRID:
ret.mass = 1668.
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 17
ret.steerActuatorDelay = 0.1
elif candidate in (CAR.FORESTER, CAR.FORESTER_2022):
ret.mass = 1568.
ret.wheelbase = 2.67
@ -107,7 +118,7 @@ class CarInterface(CarInterfaceBase):
else:
raise ValueError(f"unknown car: {candidate}")
#ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE)
#ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
if ret.openpilotLongitudinalControl:

23
selfdrive/car/subaru/values.py
Unescape View File

@ -70,6 +70,7 @@ class CAR:
IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
FORESTER = "SUBARU FORESTER 2019"
OUTBACK = "SUBARU OUTBACK 6TH GEN"
CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020"
LEGACY = "SUBARU LEGACY 7TH GEN"
FORESTER_2022 = "SUBARU FORESTER 2022"
OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN"
@ -110,6 +111,8 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
SubaruCarInfo("Subaru Crosstrek 2020-23"),
SubaruCarInfo("Subaru XV 2020-21"),
],
# TODO: is there an XV and Impreza too?
CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020"),
CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"),
CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"),
CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"),
@ -325,6 +328,24 @@ FW_VERSIONS = {
b'\xe9\xf6B0\x00',
],
},
CAR.CROSSTREK_HYBRID: {
(Ecu.abs, 0x7b0, None): [
b'\xa2 \x19e\x01',
b'\xa2 !e\x01',
],
(Ecu.eps, 0x746, None): [
b'\x9a\xc2\x01\x00',
b'\n\xc2\x01\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00el\x1f@ #',
],
(Ecu.engine, 0x7e0, None): [
b'\xd7!`@\x07',
b'\xd7!`p\a',
b'\xf4!`0\x07',
],
},
CAR.FORESTER: {
(Ecu.abs, 0x7b0, None): [
b'\xa3 \x18\x14\x00',
@ -628,6 +649,7 @@ DBC = {
CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None),
CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None),
CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None),
CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None),
CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
@ -639,3 +661,4 @@ DBC = {
LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023}
GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023}
PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018}
HYBRID_CARS = {CAR.CROSSTREK_HYBRID, }

1
selfdrive/car/tests/routes.py
Unescape View File

@ -240,6 +240,7 @@ routes = [
CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020),
CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=10),
CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3),
CarTestRoute("f4e3a0c511a076f4|2022-08-04--16-16-48", SUBARU.CROSSTREK_HYBRID, segment=2),
CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.FORESTER_2022, segment=4),
CarTestRoute("f3b34c0d2632aa83|2023-07-23--20-43-25", SUBARU.OUTBACK_2023, segment=7),
# Pre-global, dashcam

1
selfdrive/car/torque_data/substitute.yaml
Unescape View File

@ -76,6 +76,7 @@ VOLKSWAGEN POLO 6TH GEN: VOLKSWAGEN GOLF 7TH GEN
SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN
SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN
SUBARU CROSSTREK HYBRID 2020: SUBARU IMPREZA SPORT 2020
SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN
# Old subarus don't have much data guessing it's like low torque impreza

Write Preview
Loading…
Cancel
Save