From 65e3bdef752a261c77251cc5e15a1e3aa86b4728 Mon Sep 17 00:00:00 2001 From: Brad Thaler Date: Wed, 11 Jul 2018 15:54:08 -0400 Subject: [PATCH 1/4] Toyota C-HR 2018 --- selfdrive/car/toyota/carcontroller.py | 50 +++++++++++++-------------- selfdrive/car/toyota/carstate.py | 2 +- selfdrive/car/toyota/interface.py | 10 +++++- selfdrive/car/toyota/values.py | 5 +++ selfdrive/controls/lib/planner.py | 4 +-- selfdrive/pandad.py | 2 +- 6 files changed, 43 insertions(+), 30 deletions(-) mode change 100644 => 100755 selfdrive/pandad.py diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 7752add1b23c2a..b0901e27a54b68 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -199,19 +199,19 @@ def update(self, sendcan, enabled, CS, frame, actuators, if self.angle_control: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) - elif ECU.APGS in self.fake_ecus: - can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) + # elif ECU.APGS in self.fake_ecus: + # can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control - if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): - if ECU.DSU in self.fake_ecus: - can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) - else: - can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) + # if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): + # if ECU.DSU in self.fake_ecus: + # can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) + # else: + # can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) - if frame % 10 == 0 and ECU.CAM in self.fake_ecus: - for addr in TARGET_IDS: - can_sends.append(create_video_target(frame/10, addr)) + # if frame % 10 == 0 and ECU.CAM in self.fake_ecus: + # for addr in TARGET_IDS: + # can_sends.append(create_video_target(frame/10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display @@ -232,21 +232,21 @@ def update(self, sendcan, enabled, CS, frame, actuators, #*** static msgs *** - for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: - if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: - # special cases - if fr_step == 5 and ecu == ECU.CAM and bus == 1: - cnt = (((frame / 5) % 7) + 1) << 5 - vl = chr(cnt) + vl - elif addr in (0x489, 0x48a) and bus == 0: - # add counter for those 2 messages (last 4 bits) - cnt = ((frame/100)%0xf) + 1 - if addr == 0x48a: - # 0x48a has a 8 preceding the counter - cnt += 1 << 7 - vl += chr(cnt) - - can_sends.append(make_can_msg(addr, vl, bus, False)) + # for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: + # if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: + # # special cases + # if fr_step == 5 and ecu == ECU.CAM and bus == 1: + # cnt = (((frame / 5) % 7) + 1) << 5 + # vl = chr(cnt) + vl + # elif addr in (0x489, 0x48a) and bus == 0: + # # add counter for those 2 messages (last 4 bits) + # cnt = ((frame/100)%0xf) + 1 + # if addr == 0x48a: + # # 0x48a has a 8 preceding the counter + # cnt += 1 << 7 + # vl += chr(cnt) + # + # can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 28bfc0fb5761a7..99eff16bae1069 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -19,7 +19,7 @@ def parse_gear_shifter(can_gear, car_fingerprint): elif can_gear == 0x4: return "brake" elif car_fingerprint in [CAR.RAV4, CAR.RAV4H, - CAR.LEXUS_RXH, CAR.COROLLA]: + CAR.LEXUS_RXH, CAR.COROLLA, CAR.CHR]: if can_gear == 0x20: return "park" elif can_gear == 0x10: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d26992ad1ffdcc..5557206ff78feb 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -106,6 +106,14 @@ def get_params(candidate, fingerprint): ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + elif candidate == CAR.CHR: + ret.safetyParam = 100 + ret.wheelbase = 2.63906 + ret.steerRatio = 13.6 + ret.mass = 3300./2.205 + std_cargo + ret.steerKpV, ret.steerKiV = [[0.723], [0.0428]] + ret.steerKf = 0.00006 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -114,7 +122,7 @@ def get_params(candidate, fingerprint): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH]: # rav4 hybrid can do stop and go + if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR]: # rav4 hybrid can do stop and go ret.minEnableSpeed = -1. elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 4749ea247cc986..673a4b294e7e9c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -6,6 +6,7 @@ class CAR: RAV4 = "TOYOTA RAV4 2017" COROLLA = "TOYOTA COROLLA 2017" LEXUS_RXH = "LEXUS RX HYBRID 2017" + CHR = "TOYOTA C-HR 2018" class ECU: @@ -97,6 +98,9 @@ def check_ecu_msgs(fingerprint, candidate, ecu): CAR.LEXUS_RXH: [{ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 }], + CAR.CHR: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 800: 8, 810: 2, 812: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1056: 8, 1059: 1, 1114: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8 + }], } @@ -106,4 +110,5 @@ def check_ecu_msgs(fingerprint, candidate, ecu): CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.CHR: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'), } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 5beec543e77cb4..359399477dc753 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -455,8 +455,8 @@ def update(self, CS, LaC, LoC, v_cruise_kph, user_distracted): events = [] if self.model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.radar_dead or 'commIssue' in self.radar_errors: - events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + # if self.radar_dead or 'commIssue' in self.radar_errors: + # events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py old mode 100644 new mode 100755 index 7f05a078c94bd7..0320b001f7e3f9 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -4,7 +4,7 @@ from panda import ensure_st_up_to_date def main(gctx=None): - ensure_st_up_to_date() + # ensure_st_up_to_date() os.chdir("boardd") os.execvp("./boardd", ["./boardd"]) From 16f6e9bbfb4a3edd82c4ca1c7289d9aa47e7c05b Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 13 Jul 2018 20:40:51 -0700 Subject: [PATCH 2/4] generalized radar interface instead of skipping radar error checks --- selfdrive/car/toyota/radar_interface.py | 15 +++++++++++---- selfdrive/car/toyota/values.py | 2 ++ selfdrive/controls/lib/planner.py | 4 ++-- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 1568ff5fa8e063..224de33c57637c 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,11 +1,13 @@ #!/usr/bin/env python import os +import zmq +import time from selfdrive.can.parser import CANParser from cereal import car from common.realtime import sec_since_boot -import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging +from selfdrive.car.values import NO_DSU_CAR RADAR_MSGS = range(0x210, 0x220) @@ -30,15 +32,21 @@ def __init__(self, CP): self.delay = 0.0 # Delay of radar - # Nidec self.rcp = _create_radard_can_parser() + self.no_dsu = CP.carFingerprint in NO_DSU_CAR context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) def update(self): - canMonoTimes = [] + ret = car.RadarState.new_message() + if self.no_dsu: + # TODO: make a adas dbc file for dsu-less models + time.sleep(0.05) + return ret + + canMonoTimes = [] updated_messages = set() while 1: tm = int(sec_since_boot() * 1e9) @@ -47,7 +55,6 @@ def update(self): if 0x21f in updated_messages: break - ret = car.RadarState.new_message() errors = [] if not self.rcp.can_valid: errors.append("commIssue") diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 673a4b294e7e9c..e795b2e6240e40 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -112,3 +112,5 @@ def check_ecu_msgs(fingerprint, candidate, ecu): CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), CAR.CHR: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'), } + +NO_DSU_CAR = [CAR.CHR] diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 359399477dc753..5beec543e77cb4 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -455,8 +455,8 @@ def update(self, CS, LaC, LoC, v_cruise_kph, user_distracted): events = [] if self.model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - # if self.radar_dead or 'commIssue' in self.radar_errors: - # events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.radar_dead or 'commIssue' in self.radar_errors: + events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge From 075ff6ad25709c2bdb8a90ecbf8537b8c347e142 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 13 Jul 2018 20:57:35 -0700 Subject: [PATCH 3/4] typo --- selfdrive/car/toyota/radar_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 224de33c57637c..82d208f829ecb5 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -7,7 +7,7 @@ from common.realtime import sec_since_boot from selfdrive.services import service_list import selfdrive.messaging as messaging -from selfdrive.car.values import NO_DSU_CAR +from selfdrive.car.toyota.values import NO_DSU_CAR RADAR_MSGS = range(0x210, 0x220) From 246ad1a3f41ff5e6efb87435c46b740f2bf141e5 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 13 Jul 2018 21:41:41 -0700 Subject: [PATCH 4/4] carcontroller now generic and supports other toyotas. To be tested --- selfdrive/car/toyota/carcontroller.py | 54 ++++++++++++++------------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b0901e27a54b68..cc554cd15d1c8f 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,7 @@ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command -from selfdrive.car.toyota.values import ECU, STATIC_MSGS +from selfdrive.car.toyota.values import ECU, STATIC_MSGS, NO_DSU_CAR from selfdrive.can.packer import CANPacker # Accel limits @@ -106,6 +106,8 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ self.standstill_req = False self.angle_control = False + self.no_dsu = car_fingerprint in NO_DSU_CAR + self.steer_angle_enabled = False self.ipas_reset_counter = 0 @@ -199,19 +201,19 @@ def update(self, sendcan, enabled, CS, frame, actuators, if self.angle_control: can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, ECU.APGS in self.fake_ecus)) - # elif ECU.APGS in self.fake_ecus: - # can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) + elif ECU.APGS in self.fake_ecus and not self.no_dsu: + can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control - # if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): - # if ECU.DSU in self.fake_ecus: - # can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) - # else: - # can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) + if ((frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus)) and not self.no_dsu: + if ECU.DSU in self.fake_ecus: + can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req)) + else: + can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False)) - # if frame % 10 == 0 and ECU.CAM in self.fake_ecus: - # for addr in TARGET_IDS: - # can_sends.append(create_video_target(frame/10, addr)) + if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not self.no_dsu: + for addr in TARGET_IDS: + can_sends.append(create_video_target(frame/10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display @@ -232,21 +234,21 @@ def update(self, sendcan, enabled, CS, frame, actuators, #*** static msgs *** - # for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: - # if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: - # # special cases - # if fr_step == 5 and ecu == ECU.CAM and bus == 1: - # cnt = (((frame / 5) % 7) + 1) << 5 - # vl = chr(cnt) + vl - # elif addr in (0x489, 0x48a) and bus == 0: - # # add counter for those 2 messages (last 4 bits) - # cnt = ((frame/100)%0xf) + 1 - # if addr == 0x48a: - # # 0x48a has a 8 preceding the counter - # cnt += 1 << 7 - # vl += chr(cnt) - # - # can_sends.append(make_can_msg(addr, vl, bus, False)) + for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: + if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: + # special cases + if fr_step == 5 and ecu == ECU.CAM and bus == 1: + cnt = (((frame / 5) % 7) + 1) << 5 + vl = chr(cnt) + vl + elif addr in (0x489, 0x48a) and bus == 0: + # add counter for those 2 messages (last 4 bits) + cnt = ((frame/100)%0xf) + 1 + if addr == 0x48a: + # 0x48a has a 8 preceding the counter + cnt += 1 << 7 + vl += chr(cnt) + + can_sends.append(make_can_msg(addr, vl, bus, False)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())