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

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

cereal/log.capnp

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

common/params.cc

@@ -435,6 +435,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TrafficJerk", PERSISTENT},
     {"TrafficMode", PERSISTENT},
     {"TrafficModeActive", CLEAR_ON_OFFROAD_TRANSITION},
+    {"TurnDesires", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"UnlockDoors", PERSISTENT},
     {"Updated", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -995,6 +995,12 @@ class Controls:
       if lead_departing:
         self.events.add(EventName.leadDeparting)
 
+    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)
+
     if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and not self.openpilot_crashed_triggered:
       if self.random_events:
         self.events.add(EventName.openpilotCrashedRandomEvents)

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,11 @@ 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 +51,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
 
@@ -65,7 +74,14 @@ 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:
+      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
@@ -125,8 +141,14 @@ class DesireHelper:
 
     self.lane_change_completed &= one_blinker
     self.prev_one_blinker = 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):
@@ -145,6 +167,7 @@ class DesireHelper:
     is_metric = self.params.get_bool("IsMetric")
 
     lateral_tune = self.params.get_bool("LateralTune")
+    self.turn_desires = lateral_tune and self.params.get_bool("TurnDesires")
 
     self.nudgeless = self.params.get_bool("NudgelessLaneChange")
     self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0

selfdrive/controls/lib/events.py

@@ -1084,6 +1084,22 @@ EVENTS: dict[int, dict[str, 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/qt/offroad/control_settings.cc

@@ -81,6 +81,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"NNFF", tr("NNFF"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control."), ""},
     {"NNFFLite", tr("NNFF-Lite"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control for cars without available NNFF logs."), ""},
     {"TacoTune", tr("Taco Tune"), tr("Use comma's 'Taco Tune' designed for handling left and right turns."), ""},
+    {"TurnDesires", tr("Use Turn Desires"), tr("Use turn desires for greater precision in turns below the minimum lane change speed."), ""},
 
     {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
     {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""},

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -44,7 +44,7 @@ private:
   std::set<QString> deviceManagementKeys = {"DeviceShutdown", "HigherBitrate", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"};
   std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"};
   std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"};
-  std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune"};
+  std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune", "TurnDesires"};
   std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "LeadDetectionThreshold", "SmoothBraking", "StoppingDistance", "TrafficMode"};
   std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"};
   std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};

selfdrive/modeld/modeld.py

@@ -322,6 +322,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)