diff --git a/cereal/car.capnp b/cereal/car.capnp index f30b20f97..e1c292618 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -76,6 +76,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { controlsFailed @51; sensorDataInvalid @52; commIssue @53; + manualSteeringRequired @54; + manualSteeringRequiredBlinkersOn @55; } } diff --git a/common/params.py b/common/params.py index 6a0d8d86b..451a9c250 100755 --- a/common/params.py +++ b/common/params.py @@ -84,6 +84,10 @@ keys = { "DragonTempDisableSteerOnSignal": [TxType.PERSISTENT], "DragonDisableLogger": [TxType.PERSISTENT], "DragonNoctuaMode": [TxType.PERSISTENT], + "DragonCacheCar": [TxType.PERSISTENT], + "DragonCachedModel": [TxType.PERSISTENT], + "DragonCachedFP": [TxType.PERSISTENT], + "DragonCachedVIN": [TxType.PERSISTENT], } diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index f7e8c5375..5977663df 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,6 +5,9 @@ from common.fingerprints import eliminate_incompatible_cars, all_known_cars from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging +import pickle +from common.params import Params +params = Params() def get_startup_alert(car_recognized, controller_available): @@ -79,58 +82,68 @@ def fingerprint(logcan, sendcan): vin = "" frame = 0 - while True: - a = messaging.recv_one(logcan) - for can in a.can: - can_seen = True + if params.get("DragonCacheCar") == "1" and params.get("DragonCachedFP") != "" and params.get("DragonCachedModel") != "": + candidate_cars = [params.get("DragonCachedModel")] + finger = params.get("DragonCachedFP") + vin = params.get("DragonCachedVIN") + else: + while True: + a = messaging.recv_one(logcan) - # have we got a VIN query response? - if can.src == 0 and can.address == 0x7e8: - vin_never_responded = False - # basic sanity checks on ISO-TP response - if is_vin_response_valid(can.dat, vin_step, vin_cnt): - vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:] - vin_cnt += 1 - if vin_cnt == vin_cnts[vin_step]: - vin_responded = True - vin_step += 1 + for can in a.can: + can_seen = True - # ignore everything not on bus 0 and with more than 11 bits, - # which are ussually sporadic and hard to include in fingerprints. - # also exclude VIN query response on 0x7e8 - if can.src == 0 and can.address < 0x800 and can.address != 0x7e8: - finger[can.address] = len(can.dat) - candidate_cars = eliminate_incompatible_cars(can, candidate_cars) + # have we got a VIN query response? + if can.src == 0 and can.address == 0x7e8: + vin_never_responded = False + # basic sanity checks on ISO-TP response + if is_vin_response_valid(can.dat, vin_step, vin_cnt): + vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:] + vin_cnt += 1 + if vin_cnt == vin_cnts[vin_step]: + vin_responded = True + vin_step += 1 - if can_seen_frame is None and can_seen: - can_seen_frame = frame + # ignore everything not on bus 0 and with more than 11 bits, + # which are ussually sporadic and hard to include in fingerprints. + # also exclude VIN query response on 0x7e8 + if can.src == 0 and can.address < 0x800 and can.address != 0x7e8: + finger[can.address] = len(can.dat) + candidate_cars = eliminate_incompatible_cars(can, candidate_cars) - # if we only have one car choice and the time_fingerprint since we got our first - # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not - # broadcast immediately - if len(candidate_cars) == 1 and can_seen_frame is not None: - time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 - if (frame - can_seen_frame) > (time_fingerprint * 100): - break + if can_seen_frame is None and can_seen: + can_seen_frame = frame - # bail if no cars left or we've been waiting for more than 2s since can_seen - elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200): - return None, finger, "" + # if we only have one car choice and the time_fingerprint since we got our first + # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not + # broadcast immediately + if len(candidate_cars) == 1 and can_seen_frame is not None: + time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 + if (frame - can_seen_frame) > (time_fingerprint * 100): + break - # keep sending VIN qury if ECU isn't responsing. - # sendcan is probably not ready due to the zmq slow joiner syndrome - # TODO: VIN query temporarily disabled until we have the harness - if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))): - sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan')) - vin_responded = False - vin_cnt = 0 + # bail if no cars left or we've been waiting for more than 2s since can_seen + elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200): + return None, finger, "" - frame += 1 + # keep sending VIN qury if ECU isn't responsing. + # sendcan is probably not ready due to the zmq slow joiner syndrome + # TODO: VIN query temporarily disabled until we have the harness + if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))): + sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan')) + vin_responded = False + vin_cnt = 0 - # only report vin if procedure is finished - if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]: - vin = "".join(vin_dat[3:]) + frame += 1 + + # only report vin if procedure is finished + if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]: + vin = "".join(vin_dat[3:]) + + params.put("DragonCachedModel", pickle.dumps(candidate_cars)) + params.put("DragonCachedFP", pickle.dumps(finger)) + params.put("DragonCachedVIN", pickle.dumps(vin)) cloudlog.warning("fingerprinted %s", candidate_cars[0]) cloudlog.warning("VIN %s", vin) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 914c579ba..c90c1e4e4 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -72,7 +72,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "mini_car", "car", "X4", - "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"]) + "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required", "dashed_lanes"]) class CarController(object): @@ -126,7 +126,7 @@ class CarController(object): fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, - 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) + 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.lkMode) # **** process the car messages **** @@ -146,7 +146,7 @@ class CarController(object): apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) - lkas_active = enabled and not CS.steer_not_allowed + lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode # Send CAN commands. can_sends = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6c0aec52a..5cfe8fae7 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -198,6 +198,8 @@ class CarState(object): K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 + self.lkMode = True + def update(self, cp, cp_cam): # car params @@ -268,6 +270,13 @@ class CarState(object): self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] + # when user presses LKAS button on steering wheel + if self.cruise_setting == 1: + if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0: + if self.lkMode: + self.lkMode = False + else: + self.lkMode = True self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 3955bfdcd..d323afc8b 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -77,6 +77,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx): 'SET_ME_X48': 0x48, 'STEERING_REQUIRED': hud.steer_required, 'SOLID_LANES': hud.lanes, + 'DASHED_LANES': hud.dashed_lanes, 'BEEP': hud.beep, } commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 93aaf0182..9718c892f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -12,6 +12,8 @@ from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_p from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD, CAMERA_MSGS from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING +from common.params import Params +params = Params() A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -473,7 +475,11 @@ class CarInterface(object): # wait 1.0s before throwing the alert to avoid it popping when you turn off the car if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera: events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if self.CS.steer_error: + if not self.CS.lkMode: + events.append(create_event('manualSteeringRequired', [ET.WARNING])) + elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonTempDisableSteerOnSignal") == "1": + events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) + elif self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a1cffae95..dfb14e97b 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -8,7 +8,8 @@ from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_ from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness from selfdrive.swaglog import cloudlog - +from common.params import Params +params = Params() class CarInterface(object): def __init__(self, CP, CarController): @@ -340,7 +341,9 @@ class CarInterface(object): events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: + if (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonTempDisableSteerOnSignal") == "1": + events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) + elif self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.enableDsu: events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 9174cf94c..e48dfa7bf 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -661,4 +661,18 @@ ALERTS = [ "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1), + + Alert( + "manualSteeringRequired", + "STEERING REQUIRED: Lane Keeping OFF", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25), + + Alert( + "manualSteeringRequiredBlinkersOn", + "STEERING REQUIRED: Blinkers ON", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25), ] diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 542a4d624..0234337b2 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -12,6 +12,10 @@ default_conf = { 'DragonTempDisableSteerOnSignal': '0', 'DragonDisableLogger': '0', 'DragonNoctuaMode': '0', + 'DragonCacheCar': '1', + 'DragonCachedModel': '', # for cache car + 'DragonCachedFP': '', # for cache car + 'DragonCachedVIN': '', # for cache car } def dragonpilot_set_params(params):