diff --git a/cereal/car.capnp b/cereal/car.capnp index 96f6ecfe9..f703735ae 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -88,10 +88,14 @@ struct CarEvent @0x9b1657f34caf3ad3 { lowMemory @63; stockAeb @64; ldw @65; + + # dragonpilot manualSteeringRequired @66; manualSteeringRequiredBlinkersOn @67; leadCarMoving @68; leadCarDetected @69; + preAutoLaneChangeLeft @70; + preAutoLaneChangeRight @71; } } diff --git a/cereal/log.capnp b/cereal/log.capnp index aafd90bc3..b0edecaff 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -287,11 +287,12 @@ struct ThermalData { thermalStatus @14 :ThermalStatus; chargingError @17 :Bool; chargingDisabled @18 :Bool; - ipAddr @19 :Text; memUsedPercent @19 :Int8; cpuPerc @20 :Int8; + ipAddr @22 :Text; # dragonpilot + enum ThermalStatus { green @0; # all processes run yellow @1; # critical processes run (kill uploader), engage still allowed @@ -746,6 +747,8 @@ struct PathPlan { desire @17 :Desire; laneChangeState @18 :LaneChangeState; laneChangeDirection @19 :LaneChangeDirection; + # dragonpilot + autoLCAllowed @20 :Bool; enum Desire { none @0; diff --git a/common/params.py b/common/params.py index e9659b49a..61f56823d 100755 --- a/common/params.py +++ b/common/params.py @@ -155,6 +155,8 @@ keys = { "DragonEnableSRLearner": [TxType.PERSISTENT], "DragonWazeMode": [TxType.PERSISTENT], "DragonRunWaze": [TxType.PERSISTENT], + "DragonEnableAssistedLC": [TxType.PERSISTENT], + "DragonEnableAutoLC": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 954cf8306..94672b032 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -41,9 +41,15 @@ LaneChangeDirection = log.PathPlan.LaneChangeDirection def add_lane_change_event(events, path_plan): if path_plan.laneChangeState == LaneChangeState.preLaneChange: if path_plan.laneChangeDirection == LaneChangeDirection.left: - events.append(create_event('preLaneChangeLeft', [ET.WARNING])) + event_name = 'preLaneChangeLeft' + if path_plan.autoLCAllowed: + event_name = 'preAutoLaneChangeLeft' + events.append(create_event(event_name, [ET.WARNING])) else: - events.append(create_event('preLaneChangeRight', [ET.WARNING])) + event_name = 'preLaneChangeLeft' + if path_plan.autoLCAllowed: + event_name = 'preAutoLaneChangeRight' + events.append(create_event(event_name, [ET.WARNING])) elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: events.append(create_event('laneChange', [ET.WARNING])) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 4654ea917..7a2b4ad5c 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -779,4 +779,17 @@ ALERTS = [ "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25), + Alert( + "preAutoLaneChangeLeft", + "Left Auto Lane Change will engage in 3 seconds", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "preAutoLaneChangeRight", + "Right Auto Lane Change will engage in 3 seconds", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), ] diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 638cc94eb..68d1e6e2c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -8,6 +8,8 @@ from selfdrive.controls.lib.lane_planner import LanePlanner from selfdrive.config import Conversions as CV import cereal.messaging as messaging from cereal import log +# dragonpilot +from common.params import Params LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection @@ -56,6 +58,14 @@ class PathPlanner(): self.lane_change_timer = 0.0 self.prev_one_blinker = False + # dragonpilot + self.params = Params() + self.dragon_assisted_lc_enabled = False + self.dragon_auto_lc_enabled = False + self.dragon_auto_lc_allowed = False + self.dragon_auto_lc_timer = None + self.last_ts = 0. + def setup_mpc(self): self.libmpc = libmpc_py.libmpc self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) @@ -73,6 +83,13 @@ class PathPlanner(): self.angle_steers_des_time = 0.0 def update(self, sm, pm, CP, VM): + # dragonpilot + cur_time = sec_since_boot() + if cur_time - self.last_ts > 5.: + self.dragon_assisted_lc_enabled = True if self.params.get("DragonEnableAssistedLC", encoding='utf8') == "1" else False + self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False + self.last_ts = cur_time + v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active @@ -90,7 +107,11 @@ class PathPlanner(): lane_change_direction = LaneChangeDirection.none one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker - if not active or self.lane_change_timer > 10.0: + if not active: + self.lane_change_state = LaneChangeState.off + elif active and self.dragon_auto_lc_enabled and self.lane_change_timer > 13.0: + self.lane_change_state = LaneChangeState.off + elif active and self.lane_change_timer > 10.0: self.lane_change_state = LaneChangeState.off else: if sm['carState'].leftBlinker: @@ -105,10 +126,24 @@ class PathPlanner(): lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + if self.dragon_assisted_lc_enabled: + # set torque_applied to Ture if auto_lc is enabled and time is up. + if self.dragon_auto_lc_enabled and self.dragon_auto_lc_timer is not None and cur_time > self.dragon_auto_lc_timer: + torque_applied = True + + # only allow auto lane change above 40 mph / 65kph + if self.dragon_auto_lc_enabled and v_ego >= 40 * CV.MPH_TO_MS: + self.dragon_auto_lc_allowed = True + else: + self.dragon_auto_lc_allowed = False + self.dragon_auto_lc_timer = None + # State transitions - # off - if False: # self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: + if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: self.lane_change_state = LaneChangeState.preLaneChange + # only allow auto lane change above 40 mph / 65kph + if self.dragon_auto_lc_allowed: + self.dragon_auto_lc_timer = cur_time + 3. # pre elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker: @@ -123,11 +158,17 @@ class PathPlanner(): # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: self.lane_change_state = LaneChangeState.preLaneChange + self.dragon_auto_lc_allowed = False + self.dragon_auto_lc_timer = None - # Don't allow starting lane change below 45 mph - if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): + # Don't allow starting lane change below 24 mph / 40kph + if (v_ego < 24 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): self.lane_change_state = LaneChangeState.off + # always reset timer if lane change state is off + if self.lane_change_state == LaneChangeState.off: + self.dragon_auto_lc_timer = None + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 else: @@ -214,6 +255,7 @@ class PathPlanner(): plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = lane_change_direction + plan_send.pathPlan.autoLCAllowed = self.dragon_auto_lc_allowed pm.send('pathPlan', plan_send) diff --git a/selfdrive/dragonpilot/appd/appd.py b/selfdrive/dragonpilot/appd/appd.py index 43f9857cc..d2262a111 100644 --- a/selfdrive/dragonpilot/appd/appd.py +++ b/selfdrive/dragonpilot/appd/appd.py @@ -1,8 +1,7 @@ #!/usr/bin/env python3 import time -import selfdrive.messaging as messaging -from selfdrive.services import service_list +import cereal.messaging as messaging import subprocess import cereal ThermalStatus = cereal.log.ThermalData.ThermalStatus diff --git a/selfdrive/dragonpilot/dashcamd/dashcamd.py b/selfdrive/dragonpilot/dashcamd/dashcamd.py index 47ebdb366..cc9095ef6 100644 --- a/selfdrive/dragonpilot/dashcamd/dashcamd.py +++ b/selfdrive/dragonpilot/dashcamd/dashcamd.py @@ -10,8 +10,7 @@ import os import time import datetime -import selfdrive.messaging as messaging -from selfdrive.services import service_list +import cereal.messaging as messaging import subprocess from selfdrive.swaglog import cloudlog from common.params import Params diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 8bf2e4d30..cc1779e55 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -59,6 +59,8 @@ default_conf = { 'DragonEnableSRLearner': '1', 'DragonWazeMode': '0', 'DragonRunWaze': '0', + 'DragonEnableAssistedLC': '0', + 'DragonEnableAutoLC': '0', } deprecated_conf = {