diff --git a/frogpilot/controls/lib/frogpilot_events.py b/frogpilot/controls/lib/frogpilot_events.py index 8ae182b8a..deab70590 100644 --- a/frogpilot/controls/lib/frogpilot_events.py +++ b/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) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 07625ece3..9dd3ae5ff 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 79e20108b..b33d59e98 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 71f44ba45..4044b0988 100644 --- a/selfdrive/selfdrived/events.py +++ b/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(