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/custom.capnp

@@ -22,6 +22,8 @@ enum FrogPilotEvents @0xf35cc4560bbf6ec2 {
   greenLight @1;
   pedalInterceptorNoBrake @2;
   torqueNNLoad @3;
+  turningLeft @4;
+  turningRight @5;
 }
 
 struct FrogPilotLateralPlan @0xda96579883444c35 {

common/params.cc

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

selfdrive/controls/controlsd.py

@@ -340,6 +340,13 @@ class Controls:
                                                     LaneChangeState.laneChangeFinishing):
       self.events.add(EventName.laneChange)
 
+    # Handle turning
+    if not CS.standstill:
+      if self.sm['lateralPlan'].desire == Desire.turnLeft:
+        self.events.add(FrogPilotEventName.turningLeft)
+      elif self.sm['lateralPlan'].desire == Desire.turnRight:
+        self.events.add(FrogPilotEventName.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

@@ -6,6 +6,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import calculate_
 
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection
+TurnDirection = log.LateralPlan.Desire
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
@@ -31,6 +32,12 @@ DESIRES = {
   },
 }
 
+TURN_DESIRES = {
+  TurnDirection.none: log.LateralPlan.Desire.none,
+  TurnDirection.turnLeft: log.LateralPlan.Desire.turnLeft,
+  TurnDirection.turnRight: log.LateralPlan.Desire.turnRight,
+}
+
 
 class DesireHelper:
   def __init__(self):
@@ -43,7 +50,10 @@ class DesireHelper:
     self.desire = log.LateralPlan.Desire.none
 
     # FrogPilot variables
+    self.turn_direction = TurnDirection.none
+
     self.lane_change_completed = False
+    self.turn_completed = False
 
     self.lane_change_wait_timer = 0
     self.lane_width_left = 0
@@ -81,7 +91,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
+    elif one_blinker and below_lane_change_speed and frogpilot_planner.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
@@ -145,8 +162,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.LateralPlan.Desire.none
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):

selfdrive/controls/lib/events.py

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

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -209,3 +209,5 @@ class FrogPilotPlanner:
     if self.speed_limit_controller:
       self.speed_limit_controller_override = params.get_int("SLCOverride")
       SpeedLimitController.update_frogpilot_params()
+
+    self.turn_desires = params.get_bool("TurnDesires")

selfdrive/frogpilot/ui/control_settings.cc

@@ -51,6 +51,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""},
     {"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""},
     {"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""},
+
+    {"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) {