From 4b962fef63b40990a9f7c94a738a1a80c23a23e4 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 18 Oct 2019 17:58:12 +1000 Subject: [PATCH 1/3] Add Lead Car Moving Alert --- cereal/car.capnp | 1 + common/params.py | 1 + selfdrive/controls/controlsd.py | 23 ++++++++++++++++++++ selfdrive/controls/lib/alerts.py | 6 +++++ selfdrive/dragonpilot/dragonconf/__init__.py | 1 + 5 files changed, 32 insertions(+) diff --git a/cereal/car.capnp b/cereal/car.capnp index 475f789f1..976a7ac91 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -86,6 +86,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { internetConnectivityNeeded @61; manualSteeringRequired @62; manualSteeringRequiredBlinkersOn @63; + leadCarMoving @64; } } diff --git a/common/params.py b/common/params.py index ea7950b2f..8549f7e87 100755 --- a/common/params.py +++ b/common/params.py @@ -139,6 +139,7 @@ keys = { "DragonCarModel": [TxType.PERSISTENT], "DragonCarVIN": [TxType.PERSISTENT], "DragonEnableSlowOnCurve": [TxType.PERSISTENT], + "DragonEnableLeadCarMovingAlert": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8f220dadc..05e256285 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -521,6 +521,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): dragon_toyota_stock_dsu = False dragon_lat_control = True dragon_display_steering_limit_alert = True + dragon_stopped_has_lead_count = 0 + dragon_lead_car_moving_alert = False + dragon_send_lead_car_moving_alert = False while True: # dragonpilot, don't check for param too often as it's a kernel call @@ -529,6 +532,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False dragon_lat_control = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True + dragon_lead_car_moving_alert = True if params.get("DragonEnableLeadCarMovingAlert", encoding='utf8') == "1" else False ts_last_check = ts start_time = sec_since_boot() @@ -567,6 +571,25 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if dragon_lead_car_moving_alert: + # when car has a lead and is standstill and lead is barely moving, we start counting + if not CP.radarOffCan and sm['plan'].hasLead and CS.vEgo < 0.1 and 0.1 > sm['plan'].vTarget >= 0: + dragon_stopped_has_lead_count += 1 + else: + dragon_stopped_has_lead_count = 0 + + # when we detect lead car over a sec and the lead car is started moving, we are ready to send alerts + # once the condition is triggered, we want to keep the trigger + if dragon_stopped_has_lead_count >= 100 and sm['plan'].vTargetFuture >= 0.1: + dragon_send_lead_car_moving_alert = True + + # we remove alert once our car is moving + if CS.vEgo >= 0.1: + dragon_send_lead_car_moving_alert = False + + if dragon_send_lead_car_moving_alert: + events.append(create_event('leadCarMoving', [ET.WARNING])) + if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 2ffaf562c..eb2e26036 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -731,4 +731,10 @@ ALERTS = [ "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25), + Alert( + "leadCarMoving", + "LEAD CAR IS MOVING", + "Resume Driving Manually", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), ] diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 4004fbaf4..59960e469 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -49,6 +49,7 @@ default_conf = { 'DragonCarModel': '', 'DragonCarVIN': '', 'DragonEnableSlowOnCurve': '1', + 'DragonEnableLeadCarMovingAlert': '0', } deprecated_conf = { From ce54902f69603d052701cf2de73f3f83bf9cb0b7 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 21 Oct 2019 11:11:51 +1000 Subject: [PATCH 2/3] disable internet connection checking --- selfdrive/controls/controlsd.py | 4 +-- selfdrive/thermald.py | 48 ++++++++++++++++----------------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 05e256285..48c690a17 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -563,8 +563,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) - if internet_needed: - events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) + # if internet_needed: + # events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) if not dragon_toyota_stock_dsu: # Only allow engagement with brake pressed when stopped behind another stopped car diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 6a4f23c4a..6babdbb70 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -232,30 +232,30 @@ def thermald_thread(): # **** starting logic **** # Check for last update time and display alerts if needed - now = datetime.datetime.now() - try: - last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) - except (TypeError, ValueError): - last_update = now - dt = now - last_update - - if dt.days > DAYS_NO_CONNECTIVITY_MAX: - if current_connectivity_alert != "expired": - current_connectivity_alert = "expired" - params.delete("Offroad_ConnectivityNeededPrompt") - params.put("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) - elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: - remaining_time = str(DAYS_NO_CONNECTIVITY_MAX - dt.days) - if current_connectivity_alert != "prompt" + remaining_time: - current_connectivity_alert = "prompt" + remaining_time - alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) - alert_connectivity_prompt["text"] += remaining_time + " days." - params.delete("Offroad_ConnectivityNeeded") - params.put("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) - elif current_connectivity_alert is not None: - current_connectivity_alert = None - params.delete("Offroad_ConnectivityNeeded") - params.delete("Offroad_ConnectivityNeededPrompt") + # now = datetime.datetime.now() + # try: + # last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) + # except (TypeError, ValueError): + # last_update = now + # dt = now - last_update + # + # if dt.days > DAYS_NO_CONNECTIVITY_MAX: + # if current_connectivity_alert != "expired": + # current_connectivity_alert = "expired" + # params.delete("Offroad_ConnectivityNeededPrompt") + # params.put("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) + # elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + # remaining_time = str(DAYS_NO_CONNECTIVITY_MAX - dt.days) + # if current_connectivity_alert != "prompt" + remaining_time: + # current_connectivity_alert = "prompt" + remaining_time + # alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) + # alert_connectivity_prompt["text"] += remaining_time + " days." + # params.delete("Offroad_ConnectivityNeeded") + # params.put("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) + # elif current_connectivity_alert is not None: + # current_connectivity_alert = None + params.delete("Offroad_ConnectivityNeeded") + params.delete("Offroad_ConnectivityNeededPrompt") # start constellation of processes when the car starts ignition = health is not None and health.health.started From d3b7d741af81fe9fafc120b4c66436525c7b2d43 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 21 Oct 2019 11:18:17 +1000 Subject: [PATCH 3/3] remove gas/brake value modification when allow_gas is enabled --- selfdrive/car/honda/carcontroller.py | 22 +++++++++++----------- selfdrive/car/toyota/carcontroller.py | 20 ++++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 59ae644ce..ed68fa7ce 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -88,7 +88,7 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False + # self.dragon_allow_gas = False self.dragon_lat_ctrl = True def update(self, enabled, CS, frame, actuators, \ @@ -97,7 +97,7 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + # self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True # *** apply brake hysteresis *** @@ -190,15 +190,15 @@ class CarController(): # DragonAllowGas # if we detect gas pedal pressed, we do not want OP to apply gas or brake # gasPressed code from interface.py - if not CS.CP.enableGasInterceptor: - gasPressed = CS.pedal_gas > 0 - else: - gasPressed = CS.user_gas_pressed - dragon_apply_brake = apply_brake - if self.dragon_allow_gas and gasPressed: - dragon_apply_brake = 0 - apply_gas = 0 - can_sends.append(hondacan.create_brake_command(self.packer, dragon_apply_brake, pump_on, + # if not CS.CP.enableGasInterceptor: + # gasPressed = CS.pedal_gas > 0 + # else: + # gasPressed = CS.user_gas_pressed + # dragon_apply_brake = apply_brake + # if self.dragon_allow_gas and gasPressed: + # dragon_apply_brake = 0 + # apply_gas = 0 + can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) self.apply_brake_last = apply_brake diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b11b719db..8a09cbbcc 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -120,7 +120,7 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False + # self.dragon_allow_gas = False self.dragon_lat_ctrl = True self.dragon_lane_departure_warning = True @@ -130,7 +130,7 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + # self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True @@ -247,15 +247,15 @@ class CarController(): # DragonAllowGas # if we detect gas pedal pressed, we do not want OP to apply gas or brake # gasPressed code from interface.py - if CS.CP.enableGasInterceptor: - # use interceptor values to disengage on pedal press - gasPressed = CS.pedal_gas > 15 - else: - gasPressed = CS.pedal_gas > 0 + # if CS.CP.enableGasInterceptor: + # # use interceptor values to disengage on pedal press + # gasPressed = CS.pedal_gas > 15 + # else: + # gasPressed = CS.pedal_gas > 0 - if self.dragon_allow_gas and gasPressed: - apply_accel = 0 - apply_gas = 0 + # if self.dragon_allow_gas and gasPressed: + # apply_accel = 0 + # apply_gas = 0 # 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):