Merge pull request commaai#100 from xx979xx/7.6-dev

7.6 dev

cereal/car.capnp

@@ -107,9 +107,9 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     startupWhitePanda @82;
     canErrorPersistent @83;
     belowEngageSpeed @84;
-    rightLCAbsm @85;
-    leftLCAbsm @86;
-    preventLCA @87;
+    preventLaneChange @85;
+    leftBlindspot @86;
+    rightBlindspot @87;
     turningIndicatorOn @88;
     lkasButtonOff @89;
   }

cereal/log.capnp

@@ -798,7 +798,7 @@ struct PathPlan {
   desire @17 :Desire;
   laneChangeState @18 :LaneChangeState;
   laneChangeDirection @19 :LaneChangeDirection;
-  laneChangeBSM @20 :LaneChangeBSM;
+  laneChangeBlocked @20 :LaneChangeBlocked;
 
   enum Desire {
     none @0;
@@ -822,7 +822,7 @@ struct PathPlan {
     left @1;
     right @2;
   }
-  enum LaneChangeBSM {
+  enum LaneChangeBlocked {
     off @0;
     left @1;
     right @2;

selfdrive/car/hyundai/interface.py

@@ -295,11 +295,6 @@ def update(self, c, can_strings):
       events.add(EventName.turningIndicatorOn)
     if self.lkas_button_alert:
       events.add(EventName.lkasButtonOff)
-    #TODO Varible for min Speed for LCA
-    # if ret.rightBlinker and ret.rightBlindspot and ret.vEgo > (45 * CV.MPH_TO_MS):
-      # events.add(EventName.rightLCAbsm)
-    # if ret.leftBlinker and ret.leftBlindspot and ret.vEgo > (45 * CV.MPH_TO_MS):
-      # events.add(EventName.leftLCAbsm)
     if not self.CC.longcontrol and EventName.pedalPressed in events.events:
       events.events.remove(EventName.pedalPressed)
 

selfdrive/controls/controlsd.py

@@ -34,7 +34,7 @@
 Desire = log.PathPlan.Desire
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
-LaneChangeBSM = log.PathPlan.LaneChangeBSM
+LaneChangeBlocked = log.PathPlan.LaneChangeBlocked
 EventName = car.CarEvent.EventName
 
 class Controls:
@@ -140,8 +140,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.events.add(EventName.communityFeatureDisallowed, static=True)
     if self.read_only and not passive:
       self.events.add(EventName.carUnrecognized, static=True)
-    # if hw_type == HwType.whitePanda:
-    #   self.events.add(EventName.whitePandaUnsupported, static=True)
+    if hw_type == HwType.whitePanda:
+      self.events.add(EventName.whitePandaUnsupported, static=True)
 
     # controlsd is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -190,10 +190,10 @@ def update_events(self, CS):
                                         LaneChangeState.laneChangeFinishing]:
       self.events.add(EventName.laneChange)
     # lane change bsm alerts 
-    # if self.sm['pathPlan'].laneChangeBSM == laneChangeBSM.left:
-      # self.events.add(EventName.preventLCA)
-    # if self.sm['pathPlan'].LaneChangeBSM == laneChangeBSM.right:
-      # self.events.add(EventName.preventLCA)
+    if self.sm['pathPlan'].laneChangeBlocked == LaneChangeBlocked.right:
+      events.add(car.CarEvent.EventName.rightBlindspot)
+    if self.sm['pathPlan'].laneChangeBlocked == LaneChangeBlocked.left:
+      events.add(car.CarEvent.EventName.leftBlindspot)
 
     if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
       self.events.add(EventName.canError)

selfdrive/controls/lib/events.py

@@ -456,29 +456,29 @@ def calibration_incomplete_alert(CP, sm, metric):
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .1),
   },
 
-  EventName.rightLCAbsm: {
-    ET.WARNING: Alert(
-      "Vehicle in Right Lane",
-      "Waiting for Lane to be clear",
-      AlertStatus.userPrompt, AlertSize.mid,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3),
-  },
-
-  EventName.preventLCA: {
-    ET.WARNING: Alert(
-      "Vehicle in Right Lane",
-      "Waiting for Lane to be clear",
-      AlertStatus.userPrompt, AlertSize.mid,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3),
-  },
-
-  EventName.leftLCAbsm: {
-    ET.WARNING: Alert(
-      "TAKE CONTROL",
-      "Lane Change Cancelled, Lane Unsafe",
-      AlertStatus.critical, AlertSize.full,
-      Priority.HIGH, VisualAlert.none, AudibleAlert.chimeWarningRepeat, .4, 3., 3.,),
-  },
+   EventName.rightBlindspot: {
+     ET.WARNING: Alert(
+       "Vehicle in Right Lane",
+       "Waiting for Lane to be clear",
+       AlertStatus.userPrompt, AlertSize.mid,
+       Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1),
+   },
+
+   EventName.leftBlindspot: {
+     ET.WARNING: Alert(
+       "Vehicle in Left Lane",
+       "Waiting for Lane to be clear",
+       AlertStatus.userPrompt, AlertSize.mid,
+       Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1),
+   },
+
+   EventName.preventLaneChange: {
+     ET.WARNING: Alert(
+       "TAKE CONTROL",
+       "Lane Change Cancelled, Lane Unsafe",
+       AlertStatus.critical, AlertSize.full,
+       Priority.HIGH, VisualAlert.none, AudibleAlert.none, .0, .1, .1),
+   },
 
   # ********** events that affect controls state transitions **********
 

selfdrive/controls/lib/pathplanner.py

@@ -12,7 +12,7 @@
 
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
-LaneChangeBSM = log.PathPlan.LaneChangeBSM
+LaneChangeBlocked = log.PathPlan.LaneChangeBlocked
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -63,7 +63,7 @@ def __init__(self, CP):
     self.lane_change_ll_prob = 1.0
     self.prev_one_blinker = False
     self.pre_auto_LCA_timer = 0.0
-    self.lane_change_BSM = LaneChangeBSM.off
+    self.lane_change_Blocked = LaneChangeBlocked.off
     self.prev_torque_applied = False
 
   def setup_mpc(self):
@@ -103,10 +103,9 @@ def update(self, sm, pm, CP, VM):
     one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
 
-    if sm['carState'].leftBlinker:
-      self.lane_change_direction = LaneChangeDirection.left
-    elif sm['carState'].rightBlinker:
-      self.lane_change_direction = LaneChangeDirection.right
+    left_BlindSpot = sm['carState'].leftBlindspot
+    right_BlindSpot = sm['carState'].rightBlindspot
+
 
     if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled):
       self.lane_change_state = LaneChangeState.off
@@ -119,11 +118,11 @@ def update(self, sm, pm, CP, VM):
 
       if self.lane_change_direction == LaneChangeDirection.left:
         torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
-        if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not lca_left:
+        if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not left_BlindSpot:
           torque_applied = True # Enable auto LCA only once after 2 sec 
       else:
         torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
-        if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not lca_right:
+        if CP.autoLcaEnabled and 2.5 > self.pre_auto_LCA_timer > 2.0 and not right_BlindSpot:
           torque_applied = True # Enable auto LCA only once after 2 sec 
 
       lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
@@ -138,42 +137,33 @@ def update(self, sm, pm, CP, VM):
       elif self.lane_change_state == LaneChangeState.preLaneChange:
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
+          self.lane_change_Blocked = LaneChangeBlocked.off
         elif torque_applied:
-          if self.prev_torque_applied or self.lane_change_direction == LaneChangeDirection.left and not lca_left or \
-                  self.lane_change_direction == LaneChangeDirection.right and not lca_right:
+          if self.prev_torque_applied or self.lane_change_direction == LaneChangeDirection.left and not left_BlindSpot or \
+                  self.lane_change_direction == LaneChangeDirection.right and not right_BlindSpot:
             self.lane_change_state = LaneChangeState.laneChangeStarting
+            self.lane_change_Blocked = LaneChangeBlocked.off
           else:
+            if not self.prev_torque_applied:
+              if left_BlindSpot:
+                self.lane_change_Blocked = LaneChangeBlocked.left
+              elif right_BlindSpot:
+                self.lane_change_Blocked = LaneChangeBlocked.right
             if self.pre_auto_LCA_timer < 10.:
               self.pre_auto_LCA_timer = 10.
         else:
+          if not (left_BlindSpot or right_BlindSpot):
+            self.lane_change_Blocked = LaneChangeBlocked.off
           if self.pre_auto_LCA_timer > 10.3:
             self.prev_torque_applied = True
 
-      # bsm
-      elif self.lane_change_state == LaneChangeState.laneChangeStarting:
-        if lca_left and self.lane_change_direction == LaneChangeDirection.left and not self.prev_torque_applied:
-          self.lane_change_BSM = LaneChangeBSM.left
-          self.lane_change_state = LaneChangeState.preLaneChange
-        elif lca_right and self.lane_change_direction == LaneChangeDirection.right and not self.prev_torque_applied:
-          self.lane_change_BSM = LaneChangeBSM.right
-          self.lane_change_state = LaneChangeState.preLaneChange
-        else:
-          # starting
-          self.lane_change_BSM = LaneChangeBSM.off
-          if self.lane_change_state == LaneChangeState.laneChangeStarting:
-            # fade out over .5s
-            self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
-            # 98% certainty
-            if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
-              self.lane_change_state = LaneChangeState.laneChangeFinishing
-
       # starting
-      #elif self.lane_change_state == LaneChangeState.laneChangeStarting:
+      elif self.lane_change_state == LaneChangeState.laneChangeStarting:
         # fade out over .5s
-        #self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
+        self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
         # 98% certainty
-        #if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
-          #self.lane_change_state = LaneChangeState.laneChangeFinishing
+        if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
+          self.lane_change_state = LaneChangeState.laneChangeFinishing
 
       # finishing
       elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
@@ -186,12 +176,6 @@ def update(self, sm, pm, CP, VM):
 
     if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
       self.lane_change_timer = 0.0
-      if self.lane_change_BSM == LaneChangeBSM.right:
-        if not lca_right:
-          self.lane_change_BSM = LaneChangeBSM.off
-      if self.lane_change_BSM == LaneChangeBSM.left:
-        if not lca_left:
-          self.lane_change_BSM = LaneChangeBSM.off
     else:
       self.lane_change_timer += DT_MDL
 
@@ -267,7 +251,7 @@ def update(self, sm, pm, CP, VM):
     plan_send.pathPlan.desire = desire
     plan_send.pathPlan.laneChangeState = self.lane_change_state
     plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
-    plan_send.pathPlan.laneChangeBSM = self.lane_change_BSM
+    plan_send.pathPlan.laneChangeBlocked = self.lane_change_Blocked
 
     pm.send('pathPlan', plan_send)