mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
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>
old-commit-hash: 973d90be779efc0e4a83eeab213c0cc9b4d711ce
This commit is contained in:
Adeeb Shihadeh
2023-08-23 18:50:55 -07:00
committed by GitHub
parent 8c6cb78f38
commit 56facfcc5a
7 changed files with 102 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
release/files_common
+1
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
selfdrive/car/subaru/carcontroller.py
+4 -2
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,8 +101,9 @@ class CarController:
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
else:
if pcm_cancel_cmd:
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))
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))
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
selfdrive/car/subaru/carstate.py
+58 -28
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,8 +66,12 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
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
if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
@@ -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,18 +95,23 @@ 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
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"])
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
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"])
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:
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
@@ -106,44 +119,53 @@ 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),
("ES_Distance", 20),
("ES_Status", 20),
("ES_Brake", 20),
]
if CP.carFingerprint not in HYBRID_CARS:
messages += [
("ES_Distance", 20),
("ES_Status", 20)
]
return messages
@staticmethod
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:
messages.append(("BSD_RCTA", 17))
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):
if CP.carFingerprint in GLOBAL_GEN2:
messages = CarState.get_common_global_body_messages()
messages += CarState.get_common_global_es_messages()
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)
messages = []
if CP.carFingerprint in GLOBAL_GEN2:
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
selfdrive/car/subaru/interface.py
+14 -3
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:
selfdrive/car/subaru/values.py
+23
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, }
selfdrive/car/tests/routes.py
+1
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
selfdrive/car/torque_data/substitute.yaml
+1
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