mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 10:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Toyota: add 2023 RAV4/RAV4 Hybrid as dashcam (#27609)

Browse Source 
* test out lta message

* correctly send

* percentage is percentage driver isn't overriding

* closer to stock system

* should be able to send LTA at 100hz, counter is the same as LKA

* small amount of torque towards desired

* Offset commanded torque correctly

* Too little torque

* use car's SETME's

* Revert "use car's SETME's"

This reverts commit c88856969995f97cc5ec4e2b24a5cc4e3ef4721a.

* try cutting steer every second

* Didn't seem to matter

This reverts commit 6923498b2a9eed8beb3998f441899f1df4948f6f.

* steer to 0

* let packer set counter

* add for camry

* log steer faults

* comments

* bump opendbc

* add opParams

* add lta safety

* set safety param

* fix torque control bug

* bump panda

* fix missing signal

* [experiment] apply some rate limiting and anti-windup

* no faults, clip to 90 deg, decent torque blending

* clean up blending

* toyota angle rate limits

* use std angle limits

* assert we only add angle control to TSS2 cars

* clean up carcontroller a bit

* space space

* bump opendbc

* clean up toyotacan from opendbc change

* bump panda

* will tests run?

* steer at zero

* refactor angle to use LatControlPID with zero gains

* stop some faults and tuning

* possibly fix integral wind up at max torque

* Add 2023 rav4

* limit torque inside EPS when overriding (no huge windup in edge cases when overriding)

* fix wind up issue after turns (or prolonged saturation)

* this doesn't work that well

* try these limits

* try this

* log the angle!

* global variable

* Apply suggestions from code review

* clip angle to 3 m/s/s

* some tolerance for roll

* raise limits a bit

* bumppanda

* fix faults

* still not good

* offsetting causing hugging?

* Revert "offsetting causing hugging?"

This reverts commit a42ec0b772ed74a1fd54b7fef2e7c275a3e8333b.

* reduce kp

* class var not needed

* limit up angle delta

* fix saturation check

* feedforward includes offset

* some threshold for roll

* bump panda

* surely we don't need this

* test stuff

* Toyota: Add FW for 2023 RAV4 Hybrid (#27494)

* Toyota: Add FW for 2023 RAV4 Hybrid

* Set RAVH_TSS2_2022 to use angle SteerControlType

* bump panda

* remove extras

* Revert "remove extras"

This reverts commit 87378e734915c107f57f99a5feef4dcb2b2a9a37.

* no max angle limit for now

* add as separate platforms

* remove debug scripts

* revert to master

* remove these FW versions from 2022

* dashcam these cars :(

* fix test

* interface

* add to untested routes

* never send torque with LTA cars

* fix values

* clean up controlsd

* reset lat control files

* use the car param

* add to params

* bump panda to master

* Update selfdrive/car/toyota/carcontroller.py

* don't set torque params if angle control (fixes controlsd bug)

* reset controlsd

* keyword

* in another pr

* simplify test

* rm line

* Update selfdrive/car/toyota/tests/test_toyota.py

---------

Co-authored-by: crispbee <84819466+crispbee@users.noreply.github.com>
old-commit-hash: 57c4d78869ad197189427ab68c45ac8cc7bbd9d0
This commit is contained in:
Shane Smiskol
2023-03-17 00:03:21 -07:00
committed by GitHub
parent 1e9bbbd4c6
commit 472d08fc2c
7 changed files with 83 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/tests/routes.py
+2
View File
@@ -27,6 +27,8 @@ non_tested_cars = [
HYUNDAI.KIA_OPTIMA_H,
HONDA.ODYSSEY_CHN,
VOLKSWAGEN.CRAFTER_MK2, # need a route from an ACC-equipped Crafter
TOYOTA.RAV4_TSS2_2023,
TOYOTA.RAV4H_TSS2_2023,
]
CarTestRoute = namedtuple('CarTestRoute', ['route', 'car_model', 'segment'], defaults=(None,))
selfdrive/car/torque_data/override.yaml
+4
View File
@@ -7,6 +7,10 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
NISSAN LEAF 2018: [.nan, 1.5, .nan]
NISSAN ROGUE 2019: [.nan, 1.5, .nan]
# Toyota LTA also has torque
TOYOTA RAV4 2023: [.nan, 3.0, .nan]
TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan]
# Tesla has high torque
TESLA AP1 MODEL S: [.nan, 2.5, .nan]
TESLA AP2 MODEL S: [.nan, 2.5, .nan]
selfdrive/car/toyota/carcontroller.py
+5
View File
@@ -76,6 +76,11 @@ class CarController:
apply_steer_req = 0
self.steer_rate_counter = 0
# Never actuate with LKA on cars that only support LTA
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
apply_steer = 0
apply_steer_req = 0
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not CC.enabled and CS.pcm_acc_status:
selfdrive/car/toyota/interface.py
+12 -6
View File
@@ -2,7 +2,8 @@
from cereal import car
from common.conversions import Conversions as CV
from panda import Panda
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
@@ -23,13 +24,18 @@ class CarInterface(CarInterfaceBase):
if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
if candidate in ANGLE_CONTROL_CAR:
ret.dashcamOnly = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = False
steering_angle_deadzone_deg = 0.0
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
if candidate == CAR.PRIUS:
stop_and_go = True
@@ -40,9 +46,8 @@ class CarInterface(CarInterfaceBase):
# Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
steering_angle_deadzone_deg = 0.2
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate == CAR.PRIUS_V:
stop_and_go = True
@@ -102,7 +107,8 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
stop_and_go = True
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
selfdrive/car/toyota/tests/__init__.py
View File
selfdrive/car/toyota/tests/test_toyota.py
Executable
+13
View File
@@ -0,0 +1,13 @@
#!/usr/bin/env python3
import unittest
from selfdrive.car.toyota.values import TSS2_CAR, ANGLE_CONTROL_CAR
class TestToyotaInterfaces(unittest.TestCase):
def test_angle_car_set(self):
self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0)
if __name__ == "__main__":
unittest.main()
selfdrive/car/toyota/values.py
+47 -4
View File
@@ -67,8 +67,10 @@ class CAR:
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
SIENNA = "TOYOTA SIENNA 2018"
@@ -157,8 +159,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
],
CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
@@ -1459,6 +1463,23 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AJ2000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x01896634A15000\x00\x00\x00\x00',
@@ -1546,6 +1567,23 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15264283200\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AE1001\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
],
},
CAR.SIENNA: {
(Ecu.engine, 0x700, None): [
b'\x01896630832100\x00\x00\x00\x00',
@@ -2104,6 +2142,7 @@ DBC = {
CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2114,6 +2153,7 @@ DBC = {
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4H_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2128,8 +2168,8 @@ DBC = {
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
@@ -2138,10 +2178,13 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.CHR_TSS2, CAR.CHRH_TSS2}
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2}
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
# no resume button press required