Use turn desires when below the minimum lane change speed

Added toggle to use turn desires when below the minimum lane change speed for more precise turns.

cereal/car.capnp

@@ -132,6 +132,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     pedalInterceptorNoBrake @131;
     speedLimitChanged @132;
     torqueNNLoad @133;
+    turningLeft @134;
+    turningRight @135;
     vCruise69 @136;
     yourFrogTriedToKillMe @137;
 

cereal/log.capnp

@@ -323,6 +323,12 @@ enum LaneChangeDirection {
   right @2;
 }
 
+enum TurnDirection {
+  none @0;
+  turnLeft @1;
+  turnRight @2;
+}
+
 struct CanData {
   address @0 :UInt32;
   busTime @1 :UInt16;
@@ -945,6 +951,7 @@ struct ModelDataV2 {
     hardBrakePredicted @7 :Bool;
     laneChangeState @8 :LaneChangeState;
     laneChangeDirection @9 :LaneChangeDirection;
+    turnDirection @10 :TurnDirection;
 
 
     # deprecated

common/params.cc

@@ -417,6 +417,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TetheringEnabled", PERSISTENT},
     {"TrafficMode", PERSISTENT},
     {"TrafficModeActive", PERSISTENT},
+    {"TurnDesires", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"Updated", PERSISTENT},
     {"UpdateSchedule", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -455,6 +455,13 @@ class Controls:
                                                     LaneChangeState.laneChangeFinishing):
       self.events.add(EventName.laneChange)
 
+    # Handle turning
+    if not CS.standstill:
+      if self.sm['modelV2'].meta.turnDirection == Desire.turnLeft:
+        self.events.add(EventName.turningLeft)
+      elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight:
+        self.events.add(EventName.turningRight)
+
     for i, pandaState in enumerate(self.sm['pandaStates']):
       # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
       if i < len(self.CP.safetyConfigs):

selfdrive/controls/lib/desire_helper.py

@@ -5,6 +5,7 @@ from openpilot.common.realtime import DT_MDL
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = log.Desire
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
@@ -30,6 +31,12 @@ DESIRES = {
   },
 }
 
+TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
+
 
 class DesireHelper:
   def __init__(self):
@@ -45,7 +52,10 @@ class DesireHelper:
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
 
+    self.turn_direction = TurnDirection.none
+
     self.lane_change_completed = False
+    self.turn_completed = False
 
     self.lane_change_wait_timer = 0
 
@@ -66,7 +76,15 @@ class DesireHelper:
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
+      self.turn_direction = TurnDirection.none
+    elif one_blinker and below_lane_change_speed and self.turn_desires:
+      self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
+      # Set the "turn_completed" flag to prevent lane changes after completing a turn
+      self.turn_completed = True
     else:
+      # TurnDirection.turnLeft / turnRight
+      self.turn_direction = TurnDirection.none
+
       # LaneChangeState.off
       if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
         self.lane_change_state = LaneChangeState.preLaneChange
@@ -130,8 +148,14 @@ class DesireHelper:
 
     # Reset the flags
     self.lane_change_completed &= one_blinker
+    self.turn_completed &= one_blinker
 
-    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    if self.turn_direction != TurnDirection.none:
+      self.desire = TURN_DESIRES[self.turn_direction]
+    elif not self.turn_completed:
+      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    else:
+      self.desire = log.Desire.none
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
@@ -154,3 +178,5 @@ class DesireHelper:
     self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection")
     self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0
     self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange")
+
+    self.turn_desires = self.params.get_bool("TurnDesires")

selfdrive/controls/lib/events.py

@@ -1077,6 +1077,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.PERMANENT: torque_nn_load_alert,
   },
 
+  EventName.turningLeft: {
+    ET.WARNING: Alert(
+      "Turning Left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75),
+  },
+
+  EventName.turningRight: {
+    ET.WARNING: Alert(
+      "Turning Right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75),
+  },
+
   # Random Events
   EventName.accel30: {
     ET.WARNING: Alert(

selfdrive/frogpilot/ui/control_settings.cc

@@ -82,6 +82,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"SLCVisuals", "Visuals Settings", "Manage visual settings.", ""},
     {"ShowSLCOffset", "Show Speed Limit Offset", "Show the speed limit offset seperated from the speed limit in the onroad UI when using 'Speed Limit Controller'.", ""},
     {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""},
+
+    {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {

selfdrive/modeld/modeld.py

@@ -305,6 +305,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan'])
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      modelv2_send.modelV2.meta.turnDirection = DH.turn_direction
 
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
       pm.send('modelV2', modelv2_send)