Turn Desires

frogpilot/controls/lib/frogpilot_events.py

@@ -3,6 +3,7 @@ import random
 
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV
 from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import TurnDirection
 from openpilot.selfdrive.selfdrived.events import ET, EVENT_NAME, FROGPILOT_EVENT_NAME, EventName, FrogPilotEventName, Events
 
 from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, NON_DRIVING_GEARS
@@ -192,4 +193,9 @@ class FrogPilotEvents:
 
       self.previous_traffic_mode = sm["frogpilotCarState"].trafficModeEnabled
 
+    if sm["frogpilotModelV2"].turnDirection == TurnDirection.turnLeft:
+      self.events.add(FrogPilotEventName.turningLeft)
+    elif sm["frogpilotModelV2"].turnDirection == TurnDirection.turnRight:
+      self.events.add(FrogPilotEventName.turningRight)
+
     self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names)

selfdrive/controls/lib/desire_helper.py

@@ -30,6 +30,13 @@ DESIRES = {
 }
 
 # FrogPilot variables
+TurnDirection = log.Desire
+
+TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
 
 
 class DesireHelper:
@@ -131,7 +138,12 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
-    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    if lateral_active and one_blinker and below_lane_change_speed and not carstate.standstill and frogpilot_toggles.use_turn_desires:
+      self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
+      self.desire = TURN_DESIRES[self.turn_direction]
+    else:
+      self.turn_direction = TurnDirection.none
+      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):

selfdrive/modeld/modeld.py

@@ -409,6 +409,7 @@ def main(demo=False):
 
       # FrogPilot variables
       frogpilot_modelv2_send = messaging.new_message('frogpilotModelV2')
+      frogpilot_modelv2_send.frogpilotModelV2.turnDirection = DH.turn_direction
 
       pm.send('frogpilotModelV2', frogpilot_modelv2_send)
     last_vipc_frame_id = meta_main.frame_id

selfdrive/selfdrived/events.py

@@ -1204,6 +1204,22 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
 
+  FrogPilotEventName.turningLeft: {
+    ET.WARNING: Alert(
+      "Turning Left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  FrogPilotEventName.turningRight: {
+    ET.WARNING: Alert(
+      "Turning Right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
   # Random Events
   FrogPilotEventName.accel30: {
     ET.WARNING: Alert(