Merge pull request #8 from eFiniLan/dp-devel-en

Auto LC

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;
   }
 }
 

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;

common/params.py

@@ -155,6 +155,8 @@ keys = {
   "DragonEnableSRLearner": [TxType.PERSISTENT],
   "DragonWazeMode": [TxType.PERSISTENT],
   "DragonRunWaze": [TxType.PERSISTENT],
+  "DragonEnableAssistedLC": [TxType.PERSISTENT],
+  "DragonEnableAutoLC": [TxType.PERSISTENT],
 }
 
 

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]))
 

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),
 ]

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)
 

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

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

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -59,6 +59,8 @@ default_conf = {
   'DragonEnableSRLearner': '1',
   'DragonWazeMode': '0',
   'DragonRunWaze': '0',
+  'DragonEnableAssistedLC': '0',
+  'DragonEnableAutoLC': '0',
 }
 
 deprecated_conf = {