diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e94d57e..af748cb 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -927,6 +927,12 @@ class Controls: if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff: self.events.add(EventName.torqueNNLoad) + 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) + def update_frogpilot_variables(self, CS): self.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 c4cea30..682b5da 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_TIME_MAX = 10. @@ -28,6 +29,11 @@ DESIRES = { }, } +TURN_DESIRES = { + TurnDirection.none: log.Desire.none, + TurnDirection.turnLeft: log.Desire.turnLeft, + TurnDirection.turnRight: log.Desire.turnRight, +} class DesireHelper: def __init__(self): @@ -41,9 +47,12 @@ class DesireHelper: # FrogPilot variables self.lane_change_completed = False + self.turn_completed = False self.lane_change_wait_timer = 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 @@ -58,7 +67,13 @@ 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 frogpilot_toggles.turn_desires: + self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight + 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 @@ -119,8 +134,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): diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bfda671..e000772 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1013,6 +1013,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 cbc1693..1a52489 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -304,6 +304,7 @@ def main(demo=False, frogpilot_toggles=None): 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)