From 824b9f251fc9a06fda4c6c842314b54afc63526f Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Fri, 7 Jun 2024 10:24:15 -0700
Subject: [PATCH] Controls - Lateral Tuning - Use Turn Desires

Use turn desires for greater precision in turns below the minimum lane change speed.
---
 selfdrive/controls/controlsd.py         |  5 +++++
 selfdrive/controls/lib/desire_helper.py | 20 +++++++++++++++++++-
 selfdrive/controls/lib/events.py        | 16 ++++++++++++++++
 selfdrive/modeld/modeld.py              |  1 +
 4 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 9728095e9..a0d61f04b 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -906,6 +906,11 @@ class Controls:
     if self.sm.frame * DT_CTRL == 5.5 and self.CP.lateralTuning.which() == "torque" and self.CI.use_nnff:
       self.events.add(EventName.torqueNNLoad)
 
+    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)
+
   def update_frogpilot_variables(self, CS, frogpilotCarState, frogpilotPlan):
     driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py
index 8bfdb4ffb..2089bb55b 100644
--- a/selfdrive/controls/lib/desire_helper.py
+++ b/selfdrive/controls/lib/desire_helper.py
@@ -4,6 +4,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.
@@ -29,6 +30,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,6 +51,8 @@ class DesireHelper:
 
     self.lane_change_wait_timer = 0.0
 
+    self.turn_direction = TurnDirection.none
+
   def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan, frogpilot_toggles):
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
@@ -59,7 +67,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 not carstate.standstill and frogpilot_toggles.turn_desires:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
     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
@@ -123,7 +138,10 @@ class DesireHelper:
     self.lane_change_completed &= one_blinker
     self.prev_one_blinker = 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]
+    else:
+      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 4445fcf1f..bcb56bc83 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -1009,6 +1009,22 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
   EventName.torqueNNLoad: {
     ET.PERMANENT: torque_nn_load_alert,
   },
+
+  EventName.turningLeft: {
+    ET.WARNING: Alert(
+      "Turning left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75),
+  },
+
+  EventName.turningRight: {
+    ET.WARNING: Alert(
+      "Turning right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75),
+  },
 }
 
 
diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py
index d85d321b7..6fca3fd17 100755
--- a/selfdrive/modeld/modeld.py
+++ b/selfdrive/modeld/modeld.py
@@ -303,6 +303,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan'], frogpilot_toggles)
       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)